diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 496b7e6..597ab9c 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -69,6 +69,9 @@ public final class Constants { public static final PIDController Y_CONTROLLER = new PIDController(2.0, 0.0, 0.0); public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController( 15.0, 0.1, 0.3, new TrapezoidProfile.Constraints(Math.PI, Math.PI)); + + public static final double MAX_VEL = 5.0; + public static final double MAX_ACC = 5.0; // swerve configuration public static final double NEUTRAL_DEADBAND = 0.04; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 58fc9e6..5a52550 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -11,6 +11,7 @@ import java.util.ArrayList; import java.util.Arrays; import java.util.HashMap; import java.util.List; +import java.util.Objects; import javax.swing.plaf.nimbus.State; @@ -127,7 +128,17 @@ public class RobotContainer { .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); } - public Command[] buildAuto(double maxVel, double maxAccel, Object... inputs) { + /** + * Generate autonomous + * @param maxVel max velocity for the path (null to override default value of 5.0 m/s) + * @param maxAccel max acceleration for the path (null to override default value of 5.0 m/s/s) + * @param inputs strings (paths) or commands you want to run (in order) + * @return array of commands + */ + public Command[] buildAuto(Double maxVel, Double maxAccel, Object... inputs) { + + maxVel = Objects.requireNonNullElse(maxVel, SwerveDriveConstants.MAX_VEL); + maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.MAX_ACC); ArrayList commands = new ArrayList(); commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset())); @@ -174,10 +185,14 @@ public class RobotContainer { */ public Command getAutonomousCommand() { // https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki - - Command[] trajCommands = buildAuto(0.5, 0.5, "Move Forward", "Move Down"); - SequentialCommandGroup ret = new SequentialCommandGroup(trajCommands); - return (new ParallelCommandGroup(buildAuto(0.1, 0.1, ret, new InstantCommand(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.2))))); + return new ParallelCommandGroup( + buildAuto( + null, + null, + new SequentialCommandGroup(buildAuto(0.5, 0.5, "Move Forward", "Move Down")), + new InstantCommand(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.2)) + ) + ); } /**