diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e57c495..9900e90 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -28,6 +28,8 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; +import frc4388.robot.commands.AutoPath1FromCenter; +import frc4388.robot.commands.AutoPath2FromRight; import frc4388.robot.commands.DrivePositionMPAux; import frc4388.robot.commands.DriveStraightAtVelocityPID; import frc4388.robot.commands.DriveStraightToPositionMM; @@ -117,7 +119,7 @@ public class RobotContainer { /* Test Buttons */ // A driver test button new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 240)); + .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 24)); // B driver test button new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) @@ -125,7 +127,7 @@ public class RobotContainer { // Y driver test button new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whenPressed(new Wait(m_robotDrive, 0)); + .whenPressed(new Wait(m_robotDrive, 0, 0)); // X driver test button new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) @@ -190,23 +192,9 @@ public class RobotContainer { // Run path following command, then stop at the end. //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - //Runs an Autonomous command group that would shoot our preloaded balls, pick up 3 more from the trench, and shoot those - //This assumes that we are positioned against parallel to the initiation line with our back bumper lined up with the center target - return new SequentialCommandGroup(new Wait(m_robotDrive, 0), - //shoot pre-loaded 3 balls - new GotoCoordinates(m_robotDrive, 75, 44, -90), - //Start Intake Ball 1 - new GotoCoordinates(m_robotDrive, 0, 12, 0), - new GotoCoordinates(m_robotDrive, 0, 28, 0), - //Start Intake Ball 2 - new GotoCoordinates(m_robotDrive, 0, 8, 0), - new GotoCoordinates(m_robotDrive, 0, 28, 0), - //Start Intake Ball 3 - new GotoCoordinates(m_robotDrive, 0, 8, 0) - /*Shoot 3 Balls*/ ); + //return new AutoPath1FromCenter(m_robotDrive); + return new AutoPath2FromRight(m_robotDrive); - /*new GotoCoordinates(m_robotDrive, 0, 68.75, 0),*/ - //new StorageIntakeGroup(m_robotIntake, m_robotStorage), } TrajectoryConfig getTrajectoryConfig() { return new TrajectoryConfig( diff --git a/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java b/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java new file mode 100644 index 0000000..ced8a88 --- /dev/null +++ b/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 + ); + } +} diff --git a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java new file mode 100644 index 0000000..e1ebaa0 --- /dev/null +++ b/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) + ); + } +} diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinates.java index 9c3fbcf..cfa8d2e 100644 --- a/src/main/java/frc4388/robot/commands/GotoCoordinates.java +++ b/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)); } diff --git a/src/main/java/frc4388/robot/commands/Wait.java b/src/main/java/frc4388/robot/commands/Wait.java index 4288abb..580d279 100644 --- a/src/main/java/frc4388/robot/commands/Wait.java +++ b/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.