From ef62fc1119cb17c4eb819d950e09e13d77f1b334 Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Wed, 11 Mar 2020 16:46:40 -0600
Subject: [PATCH] Added a five ball auto path

Co-Authored-By: kyrarivera <kyrarivera@users.noreply.github.com>
---
 .../java/frc4388/robot/RobotContainer.java    | 22 ++++++++++-----
 .../commands/auto/FiveBallAutoMiddle.java     | 27 +++++++++++++++++++
 2 files changed, 43 insertions(+), 6 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index d3c5d05..5bb9e90 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -37,6 +37,7 @@ import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.commands.auto.DriveOffLineBackward;
 import frc4388.robot.commands.auto.DriveOffLineForward;
 import frc4388.robot.commands.auto.EightBallAutoMiddle;
+import frc4388.robot.commands.auto.FiveBallAutoMiddle;
 import frc4388.robot.commands.auto.SixBallAutoMiddle;
 import frc4388.robot.commands.InterruptSubystem;
 import frc4388.robot.commands.auto.AutoPath1FromCenter;
@@ -112,6 +113,8 @@ public class RobotContainer {
 
     DriveOffLineBackward m_driveOffLineBackward;
 
+    FiveBallAutoMiddle m_fiveBallAutoMiddle;
+
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -267,25 +270,31 @@ public class RobotContainer {
             "SixBallMidComplete"
         };
 
-        m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths, false));
+        m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths));
 
         String[] eightBallAutoMiddlePaths = new String[]{
             "EightBallMidComplete"
         };
 
-        m_eightBallAutoMiddle = new EightBallAutoMiddle(m_robotDrive, buildPaths(eightBallAutoMiddlePaths, false));
+        m_eightBallAutoMiddle = new EightBallAutoMiddle(m_robotDrive, buildPaths(eightBallAutoMiddlePaths));
 
         String[] driveOffLineForwardPaths = new String[]{
             "DriveOffLineForward"
         };
 
-        m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, buildPaths(driveOffLineForwardPaths, false));
+        m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, buildPaths(driveOffLineForwardPaths));
 
         String[] driveOffLineBackwardPaths = new String[]{
             "DriveOffLineBackward"
         };
 
-        m_driveOffLineBackward = new DriveOffLineBackward(m_robotDrive, buildPaths(driveOffLineBackwardPaths, true));
+        m_driveOffLineBackward = new DriveOffLineBackward(m_robotDrive, buildPaths(driveOffLineBackwardPaths));
+        
+        String[] fiveBallAutoMiddlePaths = new String[]{
+            "FiveBallMidComplete"
+        };
+
+        m_fiveBallAutoMiddle = new FiveBallAutoMiddle(m_robotDrive, buildPaths(fiveBallAutoMiddlePaths));
     }
 
     /**
@@ -306,7 +315,8 @@ public class RobotContainer {
             //return m_sixBallAutoMiddle.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_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
+            return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
 
         } catch (Exception e) {
             System.err.println("ERROR");
@@ -349,7 +359,7 @@ public class RobotContainer {
         return ramseteCommand;
     }
 
-    public RamseteCommand[] buildPaths(String[] paths, boolean isReversed) {
+    public RamseteCommand[] buildPaths(String[] paths) {
         RamseteCommand[] ramseteCommands = new RamseteCommand[paths.length];
         double[] times = new double[paths.length];
         Trajectory initialTrajectory;
diff --git a/src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java
new file mode 100644
index 0000000..5e5e18f
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.auto;
+
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.subsystems.Drive;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class FiveBallAutoMiddle extends SequentialCommandGroup {
+  /**
+   * Creates a new FiveBallAutoMiddle.
+   */
+  public FiveBallAutoMiddle(Drive drive, RamseteCommand[] paths) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addCommands(
+      paths[0]
+    );
+  }
+}