Skip to content

Commit

Permalink
some explanations
Browse files Browse the repository at this point in the history
  • Loading branch information
catr1xLiu committed Aug 30, 2024
1 parent 9c42718 commit 49dc0d4
Show file tree
Hide file tree
Showing 3 changed files with 20 additions and 11 deletions.
24 changes: 14 additions & 10 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -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<Auto> buildAutoChooser() {
final LoggedDashboardChooser<Auto> autoSendableChooser = new LoggedDashboardChooser<>("Select Auto");
autoSendableChooser.addDefaultOption("None", Auto.none());
Expand Down Expand Up @@ -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,
Expand All @@ -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);
}

/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,11 @@

import java.util.function.Supplier;

/**
* <h1>Custom Drive Command</h1>
* <p>The chassis will automatically face to a target on field (eg. the speaker) while the pilot controls its movements</p>
* 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<Translation2d> targetPositionSupplier;
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/utils/MapleJoystickDriveInput.java
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down

0 comments on commit 49dc0d4

Please sign in to comment.