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

Jump to bottom

8/28 (#37) #38

Merged
merged 1 commit into from
Sep 2, 2024
Merged

8/28 (#37) #38

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
27 changes: 0 additions & 27 deletions src/main/java/com/stuypulse/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,33 +41,6 @@ public void robotInit() {

@Override
public void robotPeriodic() {
if (Arm.getInstance().getState() == Arm.State.FEED
&& Arm.getInstance().atTarget()
&& !Shooter.getInstance().hasNote()
&& Intake.getInstance().hasNote()
&& Intake.getInstance().getState() != Intake.State.DEACQUIRING
) {
CommandScheduler.getInstance().schedule(new ShooterAcquireFromIntake()
.andThen(new BuzzController(robot.driver)));
}

if (Arm.getInstance().getVelocity() > Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED
&& Arm.getInstance().atIntakeShouldShootAngle()
) {
CommandScheduler.getInstance().schedule(new IntakeShoot()
.until(
() -> Arm.getInstance().getVelocity() < Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED
|| !Arm.getInstance().atIntakeShouldShootAngle()
));
}

if (Arm.getInstance().getState() == Arm.State.AMP
&& !Shooter.getInstance().hasNote()
&& Shooter.getInstance().getFeederState() != Shooter.FeederState.DEACQUIRING
) {
CommandScheduler.getInstance().schedule(new ShooterManualIntake().until(() -> Arm.getInstance().getState() != Arm.State.AMP));
}

CommandScheduler.getInstance().run();
}

Expand Down
71 changes: 52 additions & 19 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 @@ -22,6 +22,7 @@
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.ShooterAcquireFromIntakeWithRetry;
import com.stuypulse.robot.commands.shooter.ShooterFeederDeacquire;
import com.stuypulse.robot.commands.shooter.ShooterFeederShoot;
import com.stuypulse.robot.commands.shooter.ShooterFeederStop;
Expand Down Expand Up @@ -68,6 +69,7 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
import edu.wpi.first.wpilibj2.command.button.Trigger;

public class RobotContainer {

Expand Down Expand Up @@ -121,10 +123,18 @@ private void configureDefaultCommands() {
private void configureButtonBindings() {
configureOperatorBindings();
configureDriverBindings();
configureAutomaticCommandScheduling();
}

private void configureDriverBindings() {
driver.getDPadUp().onTrue(new SwerveDriveSeedFieldRelative());
driver.getDPadRight().onTrue(new SwerveDriveSeedFieldRelative());

driver.getDPadUp()
.onTrue(new ArmToPreClimb())
.onTrue(new ShooterStop())
.onTrue(new IntakeStop());

driver.getDPadDown().onTrue(new ArmToClimbing());

// intake field relative
driver.getRightTriggerButton()
Expand All @@ -147,23 +157,22 @@ private void configureDriverBindings() {
// deacquire
driver.getDPadLeft()
.whileTrue(new IntakeDeacquire())
.whileTrue(new ShooterFeederDeacquire())
.whileTrue(new LEDSet(LEDInstructions.DEACQUIRING));

// speaker align and score
// score amp
driver.getRightBumper()
.whileTrue(new ConditionalCommand(
new ArmWaitUntilAtTarget()
.withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
.andThen(new ShooterFeederDeacquire().alongWith(new LEDSet(LEDInstructions.AMP_SCORE))),
new SwerveDriveDrive(driver)
.alongWith(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
.andThen(new ShooterFeederDeacquire().alongWith(new LEDSet(LEDInstructions.AMP_SCORE)))),
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 IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
)
.andThen(new ShooterFeederShoot())
)
.alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)),
() -> Arm.getInstance().getState() == Arm.State.AMP))
Expand All @@ -179,9 +188,7 @@ private void configureDriverBindings() {
.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.isAlignedToLobFerry()))
.andThen(new ShooterFeederShoot()
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
)
.andThen(new ShooterFeederShoot())
)
.alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN))
)
Expand All @@ -198,9 +205,7 @@ private void configureDriverBindings() {
.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.isAlignedToLowFerry()))
.andThen(new ShooterFeederShoot()
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
)
.andThen(new ShooterFeederShoot())
)
.alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN))
)
Expand All @@ -217,9 +222,7 @@ private void configureDriverBindings() {
.whileTrue(new ArmToSubwooferShot().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 ShooterFeederShoot()
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
)
.andThen(new ShooterFeederShoot())
)
.whileTrue(new LEDSet(LEDInstructions.SPEAKER_MANUAL))
.onFalse(new ConditionalCommand(
Expand All @@ -234,9 +237,7 @@ private void configureDriverBindings() {
.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 IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
)
.andThen(new ShooterFeederShoot())
)
.alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN_MANUAL))
)
Expand Down Expand Up @@ -271,6 +272,38 @@ private void configureOperatorBindings() {

}

private void configureAutomaticCommandScheduling() {
// automatic handoff
new Trigger(() -> arm.getState() == Arm.State.FEED
&& arm.atTarget()
&& !shooter.hasNote()
&& intake.hasNote()
&& intake.getState() != Intake.State.DEACQUIRING)
// .onTrue(new ShooterAcquireFromIntakeWithRetry().andThen(new BuzzController(driver)));
.onTrue(new ShooterAcquireFromIntake().andThen(new BuzzController(driver)));

// feeder automatically pushes note further into shooter when its sticking too far out
new Trigger(() -> arm.getState() == Arm.State.AMP
&& !shooter.hasNote()
&& shooter.getFeederState() != Shooter.FeederState.DEACQUIRING)
.onTrue(new ShooterManualIntake().until(() -> arm.getState() != Arm.State.AMP));

// run the intake when the arm is moving up from a low angle (to prevent intake from gripping it)
new Trigger(() -> arm.getVelocity() > Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED
&& arm.atIntakeShouldShootAngle())
.onTrue(new IntakeShoot()
.until(
() -> arm.getVelocity() < Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED
|| !arm.atIntakeShouldShootAngle()
)
);

// run the intake when shooting in case the intake is holding onto the note also
new Trigger(() -> shooter.getFeederState() == Shooter.FeederState.SHOOTING
&& arm.atIntakeShouldShootAngle())
.onTrue(new IntakeShoot().until(() -> shooter.getFeederState() != Shooter.FeederState.SHOOTING));
}

/**************/
/*** AUTONS ***/
/**************/
Expand Down
4 changes: 4 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
Expand Up @@ -39,6 +39,10 @@ public LEDDefaultMode() {

private LEDInstruction getInstruction() {

if (Arm.getInstance().getState() == Arm.State.CLIMBING) {
return LEDInstructions.CLIMBING;
}

if (Arm.getInstance().getState() == Arm.State.AMP) {
return LEDInstructions.ARM_AT_AMP;
}
Expand Down
25 changes: 0 additions & 25 deletions src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntake.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,44 +12,19 @@ public class ShooterAcquireFromIntake extends Command {
private final Shooter shooter;
private final Intake intake;

private final StopWatch stopWatch;
private boolean isFeeding;

public ShooterAcquireFromIntake() {
shooter = Shooter.getInstance();
intake = Intake.getInstance();

stopWatch = new StopWatch();
isFeeding = true;

addRequirements(shooter, intake);
}

@Override
public void initialize() {
stopWatch.reset();
intake.setState(Intake.State.FEEDING);
shooter.setFeederState(Shooter.FeederState.INTAKING);
}

@Override
public void execute() {
if (isFeeding) {
if (stopWatch.getTime() > Settings.Intake.HANDOFF_TIMEOUT) {
intake.setState(Intake.State.DEACQUIRING);
isFeeding = false;
stopWatch.reset();
}
}
else {
if (stopWatch.getTime() > Settings.Intake.MINIMUM_DEACQUIRE_TIME_WHEN_STUCK && intake.hasNote()) {
intake.setState(Intake.State.FEEDING);
isFeeding = true;
stopWatch.reset();
}
}
}

@Override
public boolean isFinished() {
return shooter.hasNote();
Expand Down
64 changes: 64 additions & 0 deletions src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntakeWithRetry.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,64 @@
package com.stuypulse.robot.commands.shooter;

import com.stuypulse.robot.constants.Settings;
import com.stuypulse.robot.subsystems.intake.Intake;
import com.stuypulse.robot.subsystems.shooter.Shooter;
import com.stuypulse.stuylib.util.StopWatch;

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

public class ShooterAcquireFromIntakeWithRetry extends Command {

private final Shooter shooter;
private final Intake intake;

private final StopWatch stopWatch;
private boolean isFeeding;

public ShooterAcquireFromIntakeWithRetry() {
shooter = Shooter.getInstance();
intake = Intake.getInstance();

stopWatch = new StopWatch();
isFeeding = true;

addRequirements(shooter, intake);
}

@Override
public void initialize() {
stopWatch.reset();
intake.setState(Intake.State.FEEDING);
shooter.setFeederState(Shooter.FeederState.INTAKING);
}

@Override
public void execute() {
if (isFeeding) {
if (stopWatch.getTime() > Settings.Intake.HANDOFF_TIMEOUT) {
intake.setState(Intake.State.DEACQUIRING);
isFeeding = false;
stopWatch.reset();
}
}
else {
if (stopWatch.getTime() > Settings.Intake.MINIMUM_DEACQUIRE_TIME_WHEN_STUCK && intake.hasNote()) {
intake.setState(Intake.State.FEEDING);
isFeeding = true;
stopWatch.reset();
}
}
}

@Override
public boolean isFinished() {
return shooter.hasNote();
}

@Override
public void end(boolean interrupted) {
shooter.setFeederState(Shooter.FeederState.STOP);
intake.setState(Intake.State.STOP);
}

}
4 changes: 2 additions & 2 deletions src/main/java/com/stuypulse/robot/constants/Cameras.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ public interface Limelight {
"tower-cam",
new Pose3d(
new Translation3d(Units.inchesToMeters(-11.25), Units.inchesToMeters(+3.333797), Units.inchesToMeters(23.929362)),
new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15), Units.degreesToRadians(180))
new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(-15), Units.degreesToRadians(180))
),
"11",
3000
Expand All @@ -38,7 +38,7 @@ public interface Limelight {
"plate-cam",
new Pose3d(
new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(-4.863591), Units.inchesToMeters(19.216471)),
new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(80), Units.degreesToRadians(0))
new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(-10), Units.degreesToRadians(0))
),
"96",
3001
Expand Down
6 changes: 4 additions & 2 deletions src/main/java/com/stuypulse/robot/constants/LEDInstructions.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,8 +55,8 @@ public interface LEDInstructions {

LEDInstruction DEFAULT = LEDInstructions.OFF;

LEDInstruction FIELD_RELATIVE_INTAKING = new LEDRainbow();
LEDInstruction ROBOT_RELATIVE_INTAKING = new LEDPulseColor(SLColor.BLUE);
LEDInstruction FIELD_RELATIVE_INTAKING = new LEDPulseColor(SLColor.BLUE);
LEDInstruction ROBOT_RELATIVE_INTAKING = new LEDRainbow();
LEDInstruction DEACQUIRING = new LEDPulseColor(SLColor.GOLD);

LEDInstruction SPEAKER_ALIGN = GREEN;
Expand All @@ -71,6 +71,8 @@ public interface LEDInstructions {
LEDInstruction ARM_AT_AMP = YELLOW;
LEDInstruction AMP_SCORE = GREEN;

LEDInstruction CLIMBING = new LEDRainbow();

LEDInstruction ATTENTION = new LED694(0.01, SLColor.BLUE);

LEDInstruction CONTAINS_NOTE = RED;
Expand Down
Loading
Loading