Made both auto commands groups, and added param to wait

src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java

@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+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 AutoPath1FromCenter extends SequentialCommandGroup {
+  Drive m_drive;
+
+  /**
+   * Creates a new AutoPath1FromCenter.
+   */
+  public AutoPath1FromCenter(Drive subsystem) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    m_drive = subsystem;
+
+    addCommands(  new Wait(m_drive, 0, 1), 
+                  //shoot pre-loaded 3 balls
+                  new GotoCoordinates(m_drive, 75, 44, -90),
+                  //Start Intake Ball 1
+                  new GotoCoordinates(m_drive, 0, 12, 0),
+                  new GotoCoordinates(m_drive, 0, 28, 0),
+                  //Start Intake Ball 2
+                  new GotoCoordinates(m_drive, 0, 8, 0),
+                  new GotoCoordinates(m_drive, 0, 28, 0),
+                  //Start Intake Ball 3
+                  new GotoCoordinates(m_drive, 0, 8, 0),
+                  new Wait(m_drive, 0, 2)
+                  //Shoot 3 Balls
+                  );
+  }
+}

src/main/java/frc4388/robot/commands/AutoPath2FromRight.java

@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+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 AutoPath2FromRight extends SequentialCommandGroup {
+  Drive m_drive;
+
+  /**
+   * Creates a new AutoPath2FromRight.
+   */
+  public AutoPath2FromRight(Drive subsystem) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    m_drive = subsystem;
+    
+    addCommands(  new Wait(m_drive, 0, 1),
+                  new GotoCoordinates(m_drive, 0, 77, 0),
+                  //Start Intake Ball 1
+                  new GotoCoordinates(m_drive, 0, 8, 0),
+                  new GotoCoordinates(m_drive, 0, 28, 0),
+                  //Start Intake Ball 2
+                  new GotoCoordinates(m_drive, 0, 8, 0),
+                  //Shoot 5 Balls
+                  new GotoCoordinates(m_drive, 0, 28, 0),
+                  //Start Intake Ball 1 (second round)
+                  new GotoCoordinates(m_drive, 0, 8, 0),
+                  new Wait(m_drive, 0, 2)
+                  );
+  }
+}

src/main/java/frc4388/robot/commands/GotoCoordinates.java

@@ -44,7 +44,7 @@ public class GotoCoordinates extends SequentialCommandGroup {
 
 
     addCommands(  new TurnDegrees(m_drive, m_currentAngle),
-                  new Wait(m_drive, 0.1),
+                  new Wait(m_drive, 0, 0),
                   new DriveStraightToPositionPID(m_drive, m_hypotDist), 
                   new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
   }

src/main/java/frc4388/robot/commands/Wait.java

@@ -17,15 +17,19 @@ public class Wait extends CommandBase {
   long m_waitTime;
   long m_currentTime;
   SubsystemBase m_subsystem;
+  int m_waitNum;
+
+  int counter  = 0;
 
   /**
    * Creates a new WaitCommand.
    */
-  public Wait(SubsystemBase subsystem, double seconds) {
+  public Wait(SubsystemBase subsystem, double seconds, int waitNum) {
     // Use addRequirements() here to declare subsystem dependencies.
 
     m_waitTime = (long) (seconds * 1000);
     m_subsystem = subsystem;
+    m_waitNum = waitNum;
 
     addRequirements(m_subsystem);
   }
@@ -40,8 +44,15 @@ public class Wait extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+
+    if (counter == 0) {
+      SmartDashboard.putNumber("Wait Coordinates" + m_waitNum, m_currentTime);
+    }
+
     m_currentTime = System.currentTimeMillis();
     SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime));
+
+    counter ++;
   }
 
   // Called once the command ends or is interrupted.