Skip to content

Commit

Permalink
Adding documentation to RobotContainer
Browse files Browse the repository at this point in the history
  • Loading branch information
JediScoy committed Mar 28, 2024
1 parent dff06cc commit 0835db9
Showing 1 changed file with 24 additions and 12 deletions.
36 changes: 24 additions & 12 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -91,18 +91,25 @@ public class RobotContainer {
private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
private final Telemetry logger = new Telemetry(MaxSpeed);

// Command Group for Intake and Index
private final IntakeCommandGroup intakeGroup = new IntakeCommandGroup(index, intake);

// Command Group for Intake and Index reverse
private final IntakeRevCommandGroup intakeRevGroup = new IntakeRevCommandGroup(index, intake);

// Only Sets the flywheel to target velocity, no index
private final SetFlywheel setShooterVelocity = new SetFlywheel(shooter2, arm, shooter2.FLYWHEEL_VELOCITY);

// Only Sets the flywheel to idle velocity, no index
private final SetFlywheel setShooterIdle = new SetFlywheel(shooter2, arm, shooter2.FLYWHEEL_IDLE_VELOCITY);

// An updated version of the RevAndShootCommand
private final ShootWhenReady shootWhenReady = new ShootWhenReady(shooter2, index, notesensor);

// Autonomous version of the Shoot When Ready command that addeds notesensor checks for ending the command
private final ShootWhenReadyAuton shootWhenReadyAuton = new ShootWhenReadyAuton(shooter2, index, notesensor);

// An updated version of the RevAndShootCommand that increases "power" to shoots from the stage
private final ShootFromStage shootFromStage = new ShootFromStage(arm, index, intake, shooter2, notesensor);


Expand All @@ -117,16 +124,14 @@ public class RobotContainer {
public RobotContainer() {
Limelight lime = new Limelight();

/* Pathplanner Named Commands */
NamedCommands.registerCommand("ShootClose", new ShootClose(arm, index, intake, shooter));
NamedCommands.registerCommand("ShootClose2", shootWhenReadyAuton);
/* Pathplanner Named Commands for Auton */
NamedCommands.registerCommand("ShootClose", new ShootClose(arm, index, intake, shooter)); // Command Group
NamedCommands.registerCommand("ShootClose2", shootWhenReadyAuton); // Command for autonomous, same as below
NamedCommands.registerCommand("ShootFromStage", shootFromStage);
NamedCommands.registerCommand("Intake", new IntakeIndex(index, intake));

NamedCommands.registerCommand("Intake", new IntakeIndex(index, intake)); // Command Group
NamedCommands.registerCommand("AutoShootWhenReady", shootWhenReadyAuton);
NamedCommands.registerCommand("ShootWhenReady", shootWhenReadyAuton); // Autonomous
NamedCommands.registerCommand("ShootWhenReady", shootWhenReadyAuton); // Command for autonomous
NamedCommands.registerCommand("SetFlywheelToIdle", setShooterIdle);
// Constants.ArmConstants.ARM_MID_POSE));

/*
* Auto Chooser
Expand Down Expand Up @@ -176,14 +181,14 @@ private void configureBindings() {
// negative X (left)
));
// m_driverController.a().whileTrue(drivetrain.applyRequest(() -> brake));

/*
* m_driverController.b().whileTrue(drivetrain
* .applyRequest(() -> point
* .withModuleDirection(new Rotation2d(-m_driverController.getLeftY(),
* -m_driverController.getLeftX()))));
*/

// reset the field-centric heading
/*
* m_driverController.b().whileTrue(drivetrain.applyRequest(() -> point
* .withModuleDirection(new Rotation2d(-m_driverController.getLeftY(),
Expand All @@ -196,13 +201,20 @@ private void configureBindings() {
m_driverController.x().whileTrue(new InstantCommand(() -> arm.setArmPose(Constants.ArmConstants.ARM_AMP_POSE)));
m_driverController.rightBumper().whileTrue(new IntakeCommandGroup(index, intake));
m_driverController.leftBumper().whileTrue(new IntakeRevCommandGroup(index, intake));

/* Ideally, this is the working command for turning on the shooter and index */
m_driverController.rightTrigger().whileTrue(shootWhenReady);
// m_driverController.rightTrigger().whileTrue(setShooterVelocity); // TODO Testing purposes, remove later
// m_driverController.rightTrigger().whileTrue(autonShootWhenReady); // TODO Testing purposes, remove later

// TODO Testing purposes, remove later. This only turns on the flywheel-shooter
// m_driverController.rightTrigger().whileTrue(setShooterVelocity);

// TODO Testing purposes, remove later. This would be the ideal command used for Autonomous shooting
// m_driverController.rightTrigger().whileTrue(autonShootWhenReady);

m_driverController.leftTrigger().whileTrue(new SetIndex(index, -0.75));

// m_driverController.rightTrigger().whileTrue(new RevAndShootCommand(index, shooter)); /* Previous bindings */
// m_driverController.rightTrigger().whileFalse(new InstantCommand(() -> shooter.SetOutput(0))); /* Previous bindings */
// m_driverController.rightTrigger().whileTrue(new RevAndShootCommand(index, shooter)); /* Previous bindings from LC and TC */
// m_driverController.rightTrigger().whileFalse(new InstantCommand(() -> shooter.SetOutput(0))); /* Previous bindings from LC and TC */

/* OPERATOR BINDINGS */
m_operatorController.b().whileTrue(new SetArmClimb(arm, Constants.ArmConstants.ARM_MANUAL_POWER));
Expand Down

0 comments on commit 0835db9

Please sign in to comment.