From 51a8de1045a328e5f6aa9f738f575de94e02e7eb Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Sat, 7 Mar 2020 13:41:33 -0700 Subject: [PATCH] Fixed Waiting Time Co-Authored-By: Keenan D. Buckley --- src/main/java/frc4388/robot/Robot.java | 8 ++++--- .../java/frc4388/robot/RobotContainer.java | 24 ++++++++++++------- .../commands/auto/SixBallAutoMiddle.java | 4 +++- 3 files changed, 23 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 82c0c98..8ede29f 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/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"); } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3ebd83c..e14d267 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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); diff --git a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java index f89f6ac..f47951f 100644 --- a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java +++ b/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] );