Skip to content

Commit

Permalink
cull the chaff
Browse files Browse the repository at this point in the history
  • Loading branch information
@dspeyrer
dspeyrer committed Nov 18, 2024
1 parent 5871b1f commit a52ac28
Show file tree
Hide file tree
Showing 12 changed files with 11 additions and 1,444 deletions.
165 changes: 0 additions & 165 deletions src/main/java/frc/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,9 +6,6 @@

import com.pathplanner.lib.auto.AutoBuilder;
// pathplanner
import com.pathplanner.lib.auto.NamedCommands;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.net.PortForwarder;
// Logging
import edu.wpi.first.networktables.NetworkTable;
Expand All @@ -22,17 +19,11 @@
//Commands
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;

import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj.TimedRobot;

import frc.config.SwerveConfig;
import frc.system.*;
import frc.cmd.*;

public class Robot extends TimedRobot {
public static boolean isRedAlliance = false;
Expand All @@ -41,17 +32,13 @@ public class Robot extends TimedRobot {

// Controllers
private final CommandXboxController driver = new CommandXboxController(0);
private final CommandXboxController operator = new CommandXboxController(1);

// Subsystems
private final NetworkTable subsystemsTable = NetworkTableInstance.getDefault().getTable("Subsystems");

private final Swerve drivetrain = SwerveConfig.swerve;

private final Transit transit = new Transit(subsystemsTable);
private final Intake intake = new Intake(subsystemsTable);
private final Shooter shooter = new Shooter(subsystemsTable);
private final Pivot pivot = new Pivot(subsystemsTable);

// public Robot() {
// ! DO NOT USE
Expand All @@ -62,114 +49,6 @@ static double squareInput(double input) {
return Math.copySign(input * input, input);
}

// TODO: add comments
private class Commands {
Command rumble(RumbleType type, double strength, double duration,
CommandXboxController... controllers) {
return new Command() {
@Override
public void initialize() {
for (CommandXboxController controller : controllers)
controller.getHID().setRumble(type, strength);
}

@Override
public void end(boolean interrupted) {
for (CommandXboxController controller : controllers)
controller.getHID().setRumble(type, 0);
}
}.withTimeout(duration);
}

/**
* <ol>
* <li>Runs both the intake and the transit in.</li>
* <li>the intake will stop once the note is in the transit and then it will
* rumble the driver and operator</li>
* <li>the transit will stop when it has finished its feed in procedure (runs
* it in till it seas a note than backs it out and then runs it back in until
* it sees the note)</li>
* </ol>
*
* @return Command that requires PIVOT, INTAKE and TRANSIT
*/
Command intakeNote() {
return pivot.toIntakeUntilAimed()
.alongWith(
intake.runIn().until(() -> transit.hasNote())
.andThen(rumble(RumbleType.kBothRumble, .75, .5, driver, operator)),
transit.feedIn());
}

Command primeAmp() {
return pivot.toAmp().alongWith(shooter.shootAmp(), new Command() {
@Override
public void initialize() {
// Point the shooter towards positive y, the direction of the amp entrance.
drivetrain.setRotationOverride(Rotation2d.fromDegrees(90));
}

@Override
public void end(boolean interrupted) {
drivetrain.setRotationOverride(null);
}
});
}

Command primeSpeaker() {
return new AimSpeaker(drivetrain, pivot, shooter);
}

Command primeSubwoofer() {
return pivot.toSubwoofer().alongWith(shooter.shootSpeaker());
}

Command waitThenFeed() {
return new WaitCommand(2).andThen(transit.feedOut());
}

/**
* Currently crashes the robot code; do not use.
* TODO: Why?!
*
* @return
*/
@Deprecated
Command shoot() {
return transit.feedOut().andThen(
rumble(RumbleType.kBothRumble, .75, .5, driver, operator),
pivot.toIntakeUntilAimed());
}

// Auto Commands
Command intakeNoteAuto() {
return pivot.toIntakeUntilAimed()
.andThen(
intake.runIn().alongWith(transit.runForwards()))
.until(() -> transit.hasNote());
}

Command primeSpeakerAuto() {
return primeSpeaker().alongWith(
drivetrain.driveDutyCycle(() -> 0, () -> 0, () -> 0));
}

Command shootSpeakerAuto() {
return primeSpeakerAuto()
.alongWith(new WaitCommand(1).andThen(transit.runForwards()))
.until((() -> !transit.hasNote()));
}

Command shootSpeakerAuto2() {
return primeSpeakerAuto()
.raceWith(
new WaitUntilCommand(() -> drivetrain.isAimed() && pivot.isAimed())
.andThen(transit.runForwards().until((() -> !transit.hasNote()))));
}
}

private final Commands cmds = new Commands();

private void configButtonBindings() {
// ----------- DEFAULT COMMANDS -----------

Expand All @@ -178,8 +57,6 @@ private void configButtonBindings() {
// intake by default.
// pivot.setDefaultCommand(pivot.toIntake());

shooter.setDefaultCommand(shooter.VelocityOpenLoopCmd(true, 500));

// ----------- DRIVER CONTROLS -----------

drivetrain.setDefaultCommand(drivetrain.driveDutyCycle(
Expand All @@ -189,51 +66,17 @@ private void configButtonBindings() {
// rotational direction is not orientation-dependent.
() -> -driver.getRightX()));

driver.leftTrigger().whileTrue(cmds.intakeNote());
driver.rightTrigger().whileTrue(intake.runOut());

driver.leftBumper().onTrue(drivetrain.zeroGyro());

// driver.rightBumper().onTrue(drivetrain.zeroGyroToSubwoofer());

driver.x().whileTrue(drivetrain.brake());

// ---------- OPERATOR CONTROLS ----------

pivot.setDefaultCommand(pivot.dutyCycleCtrl(operator::getLeftY));


// TODO: make button map printout for operator
operator.leftTrigger().whileTrue(cmds.primeSpeaker());
// TODO: Pathfind to the amp using a PathfindToPose command
operator.leftBumper().whileTrue(cmds.primeAmp());

operator.rightTrigger().whileTrue(transit.runForwards());
operator.rightBumper().whileTrue(transit.runBackward());

operator.a().whileTrue(shooter.shootSpeaker());
operator.y().whileTrue(cmds.primeSubwoofer());

// Zeroes the pivot, assuming it is at intake position.
operator.start().onTrue(new InstantCommand(pivot::currentZeroingSequence));

// TODO: Add climb command
}

private void configAutos() {
configNamedCommands();
// ------------------ AUTOS ------------------

autonomousChooser.addOption("Shoot",
cmds.primeSpeaker().raceWith(
new WaitUntilCommand(() -> drivetrain.isAimed() && pivot.isAimed())
.andThen(cmds.waitThenFeed()))
// TODO: configure the next two as default commands (not working)
.andThen(shooter.stop().alongWith(pivot.toIntakeUntilAimed())));

autonomousChooser.addOption("Shoot against subwoofer",
new WaitCommand(5).andThen(cmds.primeSubwoofer().raceWith(cmds.waitThenFeed())));

autonomousChooser.addOption("Drive 3m (Red)",
drivetrain.driveVelocity(() -> -1, () -> 0, () -> 0)
.until(() -> {
Expand All @@ -243,12 +86,6 @@ private void configAutos() {
SmartDashboard.putData("auto", autonomousChooser);
}

private void configNamedCommands() {
NamedCommands.registerCommand("intake", cmds.intakeNoteAuto());
NamedCommands.registerCommand("feedIn", transit.feedIn());
NamedCommands.registerCommand("shoot", cmds.shootSpeakerAuto());
}

@Override
public void robotInit() {
// Register alliance
Expand All @@ -266,7 +103,6 @@ public void robotInit() {
}

PortForwarder.add(5800, "photonvision.local", 5800);
configNamedCommands();
autonomousChooser = AutoBuilder.buildAutoChooser();
configAutos();
}
Expand Down Expand Up @@ -325,7 +161,6 @@ public void teleopExit() {

@Override
public void testInit() {
// TODO: Run an automated full systems check
CommandScheduler.getInstance().cancelAll();
}

Expand Down
Loading

0 comments on commit a52ac28

Please sign in to comment.