Fixed Waiting Time

Co-Authored-By: Keenan D. Buckley <hfocus@users.noreply.github.com>

src/main/java/frc4388/robot/Robot.java

@@ -36,7 +36,7 @@ public class Robot extends TimedRobot {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
-    SmartDashboard.putString("Auto?", "NAH");
+    SmartDashboard.putString("Is Auto Start?", "NAH");
   }
 
   /**
@@ -64,6 +64,7 @@ public class Robot extends TimedRobot {
   @Override
   public void disabledInit() {
     m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
+    SmartDashboard.putString("Is Auto Start?", "NAH");
   }
 
   @Override
@@ -75,6 +76,7 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void autonomousInit() {
+    
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
@@ -91,8 +93,8 @@ public class Robot extends TimedRobot {
 
     // schedule the autonomous command (example)
     if (m_autonomousCommand != null) {
-      m_autonomousCommand.schedule();
-      System.err.println("Auto Start");
+      //m_autonomousCommand.schedule();
+      SmartDashboard.putString("Is Auto Start?", "YEA");
     }
   }
 

src/main/java/frc4388/robot/RobotContainer.java

@@ -90,6 +90,9 @@ public class RobotContainer {
     private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
     private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
 
+    /* Autos */
+    SixBallAutoMiddle m_sixBallAutoMiddle;
+
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -107,6 +110,9 @@ public class RobotContainer {
 
         configureButtonBindings();
 
+        /* Builds Autos */
+        buildAutos();
+
         /* Default Commands */
         // drives the robot with a two-axis input from the driver controller
 
@@ -232,6 +238,14 @@ public class RobotContainer {
             .whenReleased(new InstantCommand(() -> m_robotLime.limeOff()));
     }
 
+    public void buildAutos() {
+        String[] sixBallAutoMiddlePaths = new String[]{
+            "FirstPath0",
+            "FirstPath1"
+        };
+        m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths));
+    }
+
     /**
      * Use this to pass the autonomous command to the main {@link Robot} class.
      *
@@ -242,17 +256,9 @@ public class RobotContainer {
         TrajectoryConfig config = getTrajectoryConfig();
         //Trajectory trajectory = getTrajectory(config);
 
-        String trajectoryJSON0 = "paths/FirstPath0.wpilib.json";
-        String trajectoryJSON1 = "paths/FirstPath1.wpilib.json"; 
-
-        String[] sixBallAutoMiddlePaths = new String[]{
-            "FirstPath0",
-            "FirstPath1"
-        };
-
         // Run path following command, then stop at the end.
         //SequentialCommandGroup ramseteCommands = new SequentialCommandGroup(ramseteCommand0, ramseteCommand1);
-        return new SixBallAutoMiddle(buildPaths(sixBallAutoMiddlePaths)).andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
+        return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
 
         //return new AutoPath1FromCenter(m_robotDrive, m_robotPneumatics);
         //return new AutoPath2FromRight(m_robotDrive, m_robotPneumatics);

src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java

@@ -15,6 +15,7 @@ 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.subsystems.Drive;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
@@ -23,11 +24,12 @@ public class SixBallAutoMiddle extends SequentialCommandGroup {
   /**
    * Creates a new SixBallAutoMiddle.
    */
-  public SixBallAutoMiddle(RamseteCommand[] paths) {
+  public SixBallAutoMiddle(Drive drive, RamseteCommand[] paths) {
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());
 
     addCommands(
+      new Wait(drive, 0, 1),
       paths[0],
       paths[1]
     );