diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c1485a0..6c45f7f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -198,6 +198,13 @@ public RobotContainer() { configureAutoNamedCommands(); } + private void configureAutoNamedCommands() { + // TODO: bind your named commands during auto here + NamedCommands.registerCommand("my named command", Commands.runOnce( + () -> System.out.println("my named command executing!!!") + )); + } + private static LoggedDashboardChooser buildAutoChooser() { final LoggedDashboardChooser autoSendableChooser = new LoggedDashboardChooser<>("Select Auto"); autoSendableChooser.addDefaultOption("None", Auto.none()); @@ -257,25 +264,28 @@ private void resetFieldAndOdometryForAuto(Pose2d robotStartingPoseAtBlueAlliance * JoystickButton}. */ public void configureButtonBindings() { - System.out.println("configuring key bindings... mode:" + driverModeChooser.get()); + /* joystick drive command */ final MapleJoystickDriveInput driveInput = JoystickMode.RIGHT_HANDED.equals(driverModeChooser.get()) ? MapleJoystickDriveInput.rightHandedJoystick(driverXBox) : MapleJoystickDriveInput.leftHandedJoystick(driverXBox); - final JoystickDrive joystickDrive = new JoystickDrive( driveInput, () -> true, drive ); drive.setDefaultCommand(joystickDrive); + + /* lock chassis with x-formation */ driverXBox.x().whileTrue(Commands.run(drive::lockChassisWithXFormation, drive)); + + /* reset gyro heading manually (in case the vision does not work) */ driverXBox.start().onTrue(Commands.runOnce( () -> drive.setPose(new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), drive ).ignoringDisable(true) ); - /* aim at target and drive example, delete it for your project */ + /* TODO: aim at target and drive example, delete it for your project */ final JoystickDriveAndAimAtTarget exampleFaceTargetWhileDriving = new JoystickDriveAndAimAtTarget( driveInput, drive, FieldConstants.SPEAKER_POSITION_SUPPLIER, @@ -290,13 +300,7 @@ public void configureButtonBindings() { /* (position of AMP) */ () -> FieldConstants.toCurrentAlliancePose(new Pose2d(1.85, 7.6, Rotation2d.fromDegrees(90))) ); - } - - private void configureAutoNamedCommands() { - // bind your named commands during auto here - NamedCommands.registerCommand("my named command", Commands.runOnce( - () -> System.out.println("my named command executing!!!") - )); + driverXBox.b().whileTrue(exampleAutoAlignment); } /** diff --git a/src/main/java/frc/robot/commands/drive/JoystickDriveAndAimAtTarget.java b/src/main/java/frc/robot/commands/drive/JoystickDriveAndAimAtTarget.java index 72cee88..101f0fd 100644 --- a/src/main/java/frc/robot/commands/drive/JoystickDriveAndAimAtTarget.java +++ b/src/main/java/frc/robot/commands/drive/JoystickDriveAndAimAtTarget.java @@ -17,6 +17,11 @@ import java.util.function.Supplier; +/** + *

Custom Drive Command

+ *

The chassis will automatically face to a target on field (eg. the speaker) while the pilot controls its movements

+ * The chassis will also adjust its facing in-advance, with respect to the flight time calculated from {@link MapleShooterOptimization} (this is for shooting-on-the-move) + * */ public class JoystickDriveAndAimAtTarget extends Command { private final MapleJoystickDriveInput input; private final Supplier targetPositionSupplier; diff --git a/src/main/java/frc/robot/utils/MapleJoystickDriveInput.java b/src/main/java/frc/robot/utils/MapleJoystickDriveInput.java index 7182c1f..b8bdc13 100644 --- a/src/main/java/frc/robot/utils/MapleJoystickDriveInput.java +++ b/src/main/java/frc/robot/utils/MapleJoystickDriveInput.java @@ -12,7 +12,7 @@ import static frc.robot.constants.JoystickConfigs.*; /** - * Some optimizations to the pilot's input, including + * Some optimizations to the pilot's input, including a linear dead band and * */ public class MapleJoystickDriveInput { public final DoubleSupplier joystickXSupplier, joystickYSupplier, joystickOmegaSupplier;