diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index dfe4926..1ccad70 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -21,7 +21,7 @@ import frc4388.utility.LEDPatterns; * constants are needed, to reduce verbosity. */ public final class Constants { - public static final int SELECTED_AUTO = 1; + public static final int SELECTED_AUTO = 0; public static final class DriveConstants { /* Drive Train IDs */ diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index dfe2cae..60b786e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -29,6 +29,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; +import frc4388.robot.commands.AutoPath2FromRight; import frc4388.robot.commands.CalibrateShooter; import frc4388.robot.commands.DrivePositionMPAux; import frc4388.robot.commands.DriveStraightAtVelocityPID; @@ -228,6 +229,7 @@ public class RobotContainer { RamseteCommand ramseteCommand = getRamseteCommand(trajectory); // Run path following command, then stop at the end. //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + //return new AutoPath1FromCenter(m_robotDrive, m_robotPneumatics); //return new AutoPath2FromRight(m_robotDrive, m_robotPneumatics); @@ -240,7 +242,6 @@ public class RobotContainer { } return new InstantCommand(); - // return new DrivePositionMPAux(m_robotDrive, 500.0, 12.0, 2, 60.0, 0.0); } TrajectoryConfig getTrajectoryConfig() { return new TrajectoryConfig( diff --git a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java index 43657c3..31ce8a3 100644 --- a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java +++ b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java @@ -36,10 +36,17 @@ public class AutoPath2FromRight extends SequentialCommandGroup { new GotoCoordinates(m_drive, m_pneumatics, 0, 8), //Shoot 5 Balls new GotoCoordinates(m_drive, m_pneumatics, 0, 28), - //Start Intake Ball 1 (second round) + //Start Intake Ball 6 (Ball 1 second round) new GotoCoordinates(m_drive, m_pneumatics, 0, 8), - //Start Moving to 4th Ball - new GotoCoordinates(m_drive, m_pneumatics, 60, -50), + //Move to 7th Ball + new GotoCoordinates(m_drive, m_pneumatics, 86.7, -64.11, -180), + //Move to 8th Ball + new GotoCoordinates(m_drive, m_pneumatics, -6.34, 15.31, 90), + //Move to 9th Ball + new GotoCoordinates(m_drive, m_pneumatics, 7.11, 24.41, 0), + //Move to 10th Ball + new GotoCoordinates(m_drive, m_pneumatics, -6.34, 13.30), + //Shoot 5 more Balls (Total 10 Ball Autonomous Path) new Wait(m_drive, 0, 2) ); }