diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 40f69d1..cb582c9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -281,7 +281,6 @@ public class RobotContainer { } public void buildAutos() { - System.out.println("BUILDING AUTOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOS"); String[] sixBallAutoMiddlePaths = new String[]{ "SixBallMidComplete" }; @@ -353,19 +352,22 @@ public class RobotContainer { SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto); //return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - //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)); - //return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - //return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - //return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); - return new SequentialCommandGroup( + // 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)); + // return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + // return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + // return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); + return new SequentialCommandGroup( m_sixBallAuto0, + new InstantCommand(() -> resetOdometry(new Pose2d())), + //new Wait(m_robotDrive, 0.5, 0), m_sixBallAuto1 - /**new ParallelRaceGroup( - m_sixBallAuto0, - new RunIntake(m_robotIntake) - )**/); //ParallelRaceGroup(m_sixBallAuto0, new RunIntake(m_robotIntake)); + // /**new ParallelRaceGroup( + // m_sixBallAuto0, + // new RunIntake(m_robotIntake) + // )**/); //ParallelRaceGroup(m_sixBallAuto0, new RunIntake(m_robotIntake)); + ); } catch (Exception e) { System.err.println("ERROR"); } diff --git a/src/main/java/frc4388/robot/commands/auto/Wait.java b/src/main/java/frc4388/robot/commands/auto/Wait.java index 6bbedf3..2b5a4e0 100644 --- a/src/main/java/frc4388/robot/commands/auto/Wait.java +++ b/src/main/java/frc4388/robot/commands/auto/Wait.java @@ -8,6 +8,7 @@ package frc4388.robot.commands.auto; import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Wait extends CommandBase {