Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Leds #25

Merged
merged 4 commits into from
Aug 2, 2024
Merged

Leds #25

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
c78ff38
added LED code
IanShiii Aug 2, 2024
c2e2ea9
update shooter commands, started corresponding led colors
IanShiii Aug 2, 2024
f00be4f
add corresponding led commands to robotcontainer, update ledinstructi…
IanShiii Aug 2, 2024
e92be28
make feed angle an acceptable range
IanShiii Aug 2, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
89 changes: 77 additions & 12 deletions src/main/java/com/stuypulse/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,10 +16,13 @@
import com.stuypulse.robot.commands.intake.IntakeAcquire;
import com.stuypulse.robot.commands.intake.IntakeDeacquire;
import com.stuypulse.robot.commands.intake.IntakeStop;
import com.stuypulse.robot.commands.leds.LEDDefaultMode;
import com.stuypulse.robot.commands.leds.LEDReset;
import com.stuypulse.robot.commands.leds.LEDSet;
import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntake;
import com.stuypulse.robot.commands.shooter.ShooterFeederDeacquire;
import com.stuypulse.robot.commands.shooter.ShooterFeederShoot;
import com.stuypulse.robot.commands.shooter.ShooterFeederStop;
import com.stuypulse.robot.commands.shooter.ShooterScoreAmp;
import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker;
import com.stuypulse.robot.commands.shooter.ShooterSetRPM;
import com.stuypulse.robot.commands.shooter.ShooterStop;
Expand All @@ -33,14 +36,11 @@
import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedLobFerry;
import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedLowFerry;
import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedManualLobFerry;
import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedManualLowFerry;
import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedSpeaker;
import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndLobFerry;
import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndLobFerryManual;
import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndLowFerry;
import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndLowFerryManual;
import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndScoreSpeaker;
import com.stuypulse.robot.commands.swerve.noteAlignment.SwerveDriveDriveToNote;
import com.stuypulse.robot.commands.swerve.SwerveDriveSeedFieldRelative;
import com.stuypulse.robot.constants.LEDInstructions;
import com.stuypulse.robot.constants.Ports;
import com.stuypulse.robot.constants.Settings;
import com.stuypulse.robot.constants.Settings.Driver;
Expand All @@ -51,10 +51,15 @@
import com.stuypulse.robot.subsystems.swerve.Telemetry;
import com.stuypulse.robot.subsystems.vision.AprilTagVision;
import com.stuypulse.robot.subsystems.vision.NoteVision;
import com.stuypulse.robot.util.SLColor;
import com.stuypulse.robot.util.ShooterLobFerryInterpolation;
import com.stuypulse.robot.util.ShooterSpeeds;
import com.stuypulse.robot.subsystems.arm.Arm;
import com.stuypulse.robot.subsystems.intake.Intake;
import com.stuypulse.robot.subsystems.leds.LEDController;
import com.stuypulse.robot.subsystems.leds.instructions.LEDInstruction;
import com.stuypulse.robot.subsystems.leds.instructions.LEDPulseColor;
import com.stuypulse.robot.subsystems.leds.instructions.LEDRainbow;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
Expand Down Expand Up @@ -82,6 +87,8 @@ public class RobotContainer {
public final Arm arm = Arm.getInstance();
public final SwerveDrive swerve = SwerveDrive.getInstance();

public final LEDController leds = LEDController.getInstance();

private final Telemetry logger = new Telemetry();

// Autons
Expand All @@ -106,6 +113,7 @@ public RobotContainer() {

private void configureDefaultCommands() {
swerve.setDefaultCommand(new SwerveDriveDrive(driver));
leds.setDefaultCommand(new LEDDefaultMode());
}

/***************/
Expand All @@ -125,20 +133,23 @@ private void configureDriverBindings() {
.onTrue(new ArmToFeed())
.whileTrue(new SwerveDriveDriveToNote(driver))
.whileTrue(new IntakeAcquire()
.deadlineWith(new LEDSet(LEDInstructions.INTAKING))
.andThen(new BuzzController(driver))
);

// intake robot relative
driver.getLeftTriggerButton()
.onTrue(new ArmToFeed())
.whileTrue(new IntakeAcquire()
.deadlineWith(new LEDSet(LEDInstructions.INTAKING))
.andThen(new BuzzController(driver))
)
.whileTrue(new SwerveDriveDriveRobotRelative(driver));

// deacquire
driver.getDPadLeft()
.whileTrue(new IntakeDeacquire())
.onTrue(new IntakeDeacquire())
.whileTrue(new LEDSet(LEDInstructions.DEACQUIRING))
.onFalse(new IntakeStop());

// speaker align and score
Expand All @@ -147,14 +158,18 @@ private void configureDriverBindings() {
.whileTrue(new ConditionalCommand(
new ArmWaitUntilAtTarget()
.withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
.andThen(new ShooterScoreAmp()),
.andThen(new ShooterFeederDeacquire()),
new SwerveDriveDriveAlignedSpeaker(driver)
.alongWith(new ArmToSpeaker().alongWith(new ShooterSetRPM(Settings.Shooter.SPEAKER))
.andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
.alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)))
.andThen(new WaitUntilCommand(() -> swerve.isAlignedToSpeaker()))
.andThen(new ShooterFeederShoot())
),
)
.alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)
.until(() -> swerve.isAlignedToSpeaker())
.andThen(new LEDSet(LEDInstructions.IS_ALIGNED))
),
() -> Arm.getInstance().getState() == Arm.State.AMP))
.onFalse(new ConditionalCommand(
new ShooterFeederStop(),
Expand All @@ -171,6 +186,10 @@ private void configureDriverBindings() {
.andThen(new WaitUntilCommand(() -> swerve.isAlignedToLobFerry()))
.andThen(new ShooterFeederShoot())
)
.alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN)
.until(() -> swerve.isAlignedToLobFerry())
.andThen(new LEDSet(LEDInstructions.IS_ALIGNED))
)
)
.onFalse(new ConditionalCommand(
new ShooterFeederStop(),
Expand All @@ -185,6 +204,10 @@ private void configureDriverBindings() {
.andThen(new WaitUntilCommand(() -> swerve.isAlignedToLowFerry()))
.andThen(new ShooterFeederShoot())
)
.alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN)
.until(() -> swerve.isAlignedToLowFerry())
.andThen(new LEDSet(LEDInstructions.IS_ALIGNED))
)
)
.onFalse(new ConditionalCommand(
new ShooterFeederStop(),
Expand All @@ -194,28 +217,56 @@ private void configureDriverBindings() {
// arm to amp and alignment
driver.getLeftBumper()
.onTrue(new ArmToAmp())
.onTrue(new SwerveDriveDriveAlignedAmp(driver));
.onTrue(new SwerveDriveDriveAlignedAmp(driver)
.onlyWhile(() ->
Math.abs(driver.getRightX()) < Settings.Driver.Turn.DISABLE_ALIGNMENT_DEADBAND.getAsDouble() &&
Arm.getInstance().getState() == Arm.State.AMP)
.deadlineWith(new LEDSet(LEDInstructions.AMP_WITH_ALIGN)));

// manual speaker at subwoofer
// rebind to a button on the back later
driver.getRightMenuButton()
.whileTrue(new ArmToSubwooferShot()
.andThen(new ArmWaitUntilAtTarget())
.andThen(new ShooterScoreSpeaker()))
.whileTrue(new LEDSet(LEDInstructions.SPEAKER_MANUAL))
.onFalse(new ConditionalCommand(
new ShooterFeederStop(),
new ShooterStop(),
() -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));

// manual ferry
driver.getTopButton()
.whileTrue(new SwerveDriveDriveAndLobFerryManual(driver))
.whileTrue(new SwerveDriveDriveAlignedManualLobFerry(driver)
.alongWith(new ArmToLobFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds()))
.andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
.alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)))
.andThen(new WaitUntilCommand(() -> swerve.isAlignedToManualLobFerry()))
.andThen(new ShooterFeederShoot())
)
.alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN_MANUAL)
.until(() -> swerve.isAlignedToManualLobFerry())
.andThen(new LEDSet(LEDInstructions.IS_ALIGNED))
)
)
.onFalse(new ConditionalCommand(
new ShooterFeederStop(),
new ShooterStop(),
() -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));

driver.getLeftButton()
.whileTrue(new SwerveDriveDriveAndLowFerryManual(driver))
.whileTrue(new SwerveDriveDriveAlignedManualLowFerry(driver)
.alongWith(new ArmToLowFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds()))
.andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
.alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)))
.andThen(new WaitUntilCommand(() -> swerve.isAlignedToManualLowFerry()))
.andThen(new ShooterFeederShoot())
)
.alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN_MANUAL)
.until(() -> swerve.isAlignedToManualLowFerry())
.andThen(new LEDSet(LEDInstructions.IS_ALIGNED))
)
)
.onFalse(new ConditionalCommand(
new ShooterFeederStop(),
new ShooterStop(),
Expand All @@ -229,6 +280,12 @@ private void configureDriverBindings() {
private void configureOperatorBindings() {
operator.getLeftTriggerButton().whileTrue(new IntakeDeacquire());
operator.getRightTriggerButton().whileTrue(new IntakeAcquire());

operator.getLeftMenuButton().whileTrue(new LEDSet(LEDInstructions.BLUE));
operator.getRightMenuButton().whileTrue(new LEDSet(LEDInstructions.GREEN));
operator.getDPadUp().whileTrue(new LEDSet(LEDInstructions.PINK));
operator.getDPadLeft().whileTrue(new LEDSet(LEDInstructions.RAINBOW));
operator.getDPadRight().whileTrue(new LEDSet(new LEDPulseColor(SLColor.RED, SLColor.WHITE)));
}

/**************/
Expand All @@ -244,4 +301,12 @@ public void configureAutons() {
public Command getAutonomousCommand() {
return autonChooser.getSelected();
}

public static String getAutonomousCommandNameStatic() {
if (autonChooser.getSelected() == null) {
return "Do Nothing";
}

return autonChooser.getSelected().getName();
}
}
64 changes: 64 additions & 0 deletions src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,64 @@
/************************ PROJECT IZZI *************************/
/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved. */
/* Use of this source code is governed by an MIT-style license */
/* that can be found in the repository LICENSE file. */
/***************************************************************/

package com.stuypulse.robot.commands.leds;

import com.stuypulse.robot.constants.Field;
import com.stuypulse.robot.constants.LEDInstructions;
import com.stuypulse.robot.subsystems.arm.Arm;
import com.stuypulse.robot.subsystems.intake.Intake;
import com.stuypulse.robot.subsystems.leds.LEDController;
import com.stuypulse.robot.subsystems.leds.instructions.LEDInstruction;
import com.stuypulse.robot.subsystems.shooter.Shooter;
import com.stuypulse.robot.subsystems.swerve.SwerveDrive;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;

public class LEDDefaultMode extends Command {

private final LEDController leds;
private final SwerveDrive swerve;
private final Intake intake;
private final Shooter shooter;
private final Arm arm;

public LEDDefaultMode() {
leds = LEDController.getInstance();
swerve = SwerveDrive.getInstance();
intake = Intake.getInstance();
shooter = Shooter.getInstance();
arm = Arm.getInstance();

addRequirements(leds);
}

private LEDInstruction getInstruction() {

switch (arm.getState()) {
case PRE_CLIMB:
return LEDInstructions.ARM_PRECLIMB;
case CLIMBING:
return LEDInstructions.ARM_POSTCLIMB;
case AMP:
return LEDInstructions.AMP_WITHOUT_ALIGN;
default:
break;
}

if (intake.hasNote() || shooter.hasNote()) {
return LEDInstructions.CONTAINS_NOTE;
}

return LEDInstructions.DEFAULT;
}

@Override
public void execute() {
leds.runLEDInstruction(getInstruction());
}
}
33 changes: 33 additions & 0 deletions src/main/java/com/stuypulse/robot/commands/leds/LEDReset.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
/************************ PROJECT IZZI *************************/
/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved. */
/* Use of this source code is governed by an MIT-style license */
/* that can be found in the repository LICENSE file. */
/***************************************************************/

package com.stuypulse.robot.commands.leds;

import com.stuypulse.robot.constants.LEDInstructions;
import com.stuypulse.robot.subsystems.leds.LEDController;

import edu.wpi.first.wpilibj2.command.InstantCommand;

public class LEDReset extends InstantCommand {

@Override
public boolean runsWhenDisabled() {
return true;
}

private final LEDController ledController;

public LEDReset() {
this.ledController = LEDController.getInstance();

addRequirements(ledController);
}

@Override
public void initialize() {
ledController.runLEDInstruction(LEDInstructions.DEFAULT);
}
}
35 changes: 35 additions & 0 deletions src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
/************************ PROJECT IZZI *************************/
/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved. */
/* Use of this source code is governed by an MIT-style license */
/* that can be found in the repository LICENSE file. */
/***************************************************************/

package com.stuypulse.robot.commands.leds;

import com.stuypulse.robot.subsystems.leds.LEDController;
import com.stuypulse.robot.subsystems.leds.instructions.LEDInstruction;

import edu.wpi.first.wpilibj2.command.Command;

public class LEDSet extends Command {

@Override
public boolean runsWhenDisabled() {
return true;
}

private final LEDInstruction ledInstruction;
private final LEDController ledController;

public LEDSet(LEDInstruction ledInstruction) {
this.ledInstruction = ledInstruction;
this.ledController = LEDController.getInstance();

addRequirements(ledController);
}

@Override
public void execute() {
ledController.runLEDInstruction(ledInstruction);
}
}
18 changes: 0 additions & 18 deletions src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerry.java
View file
Open in desktop

This file was deleted.

Loading
Loading