Skip to content

Commit

Permalink
Merge pull request #22 from StuyPulse/se/controls-redo
Browse files Browse the repository at this point in the history
Se/controls redo
  • Loading branch information
IanShiii authored Jul 30, 2024
2 parents 4d1dc28 + fcb95b2 commit acd3e05
Show file tree
Hide file tree
Showing 41 changed files with 958 additions and 415 deletions.
2 changes: 1 addition & 1 deletion src/main/java/com/stuypulse/robot/Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ public void robotInit() {

@Override
public void robotPeriodic() {
if (Arm.getInstance().getActualState() == Arm.State.FEED
if (Arm.getInstance().getState() == Arm.State.FEED
&& Arm.getInstance().atTarget()
&& !Shooter.getInstance().hasNote()
&& Intake.getInstance().hasNote()
Expand Down
127 changes: 78 additions & 49 deletions src/main/java/com/stuypulse/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -2,35 +2,41 @@

import com.ctre.phoenix6.Utils;
import com.stuypulse.robot.commands.BuzzController;
import com.stuypulse.robot.commands.arm.ArmDisableOverride;
import com.stuypulse.robot.commands.arm.ArmEnableOverride;
import com.stuypulse.robot.commands.arm.ArmSetShootHeightToHigh;
import com.stuypulse.robot.commands.arm.ArmSetShootHeightToLow;
import com.stuypulse.robot.commands.arm.ArmToAmp;
import com.stuypulse.robot.commands.arm.ArmToClimbing;
import com.stuypulse.robot.commands.arm.ArmToFeed;
import com.stuypulse.robot.commands.arm.ArmToFerry;
import com.stuypulse.robot.commands.arm.ArmToLobFerry;
import com.stuypulse.robot.commands.arm.ArmToLowFerry;
import com.stuypulse.robot.commands.arm.ArmToPreClimb;
import com.stuypulse.robot.commands.arm.ArmToSpeaker;
import com.stuypulse.robot.commands.arm.ArmToSpeakerHigh;
import com.stuypulse.robot.commands.arm.ArmToSpeaker;
import com.stuypulse.robot.commands.arm.ArmToSpeakerLow;
import com.stuypulse.robot.commands.arm.ArmToStow;
import com.stuypulse.robot.commands.arm.ArmToSubwooferShot;
import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget;
import com.stuypulse.robot.commands.auton.DoNothingAuton;
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.shooter.ShooterAcquireFromIntake;
import com.stuypulse.robot.commands.shooter.ShooterAutoShoot;
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.swerve.SwerveDriveAutoAlignment;
import com.stuypulse.robot.commands.shooter.ShooterStop;
import com.stuypulse.robot.commands.swerve.SwerveDriveDrive;
import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedLowFerry;
import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedSpeakerHigh;
import com.stuypulse.robot.commands.swerve.SwerveDriveDriveRobotRelative;
import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToChain;
import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToClimb;
import com.stuypulse.robot.commands.swerve.SwerveDriveXMode;
import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedSpeakerLow;
import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedAmp;
import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedLowFerry;
import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedSpeakerHigh;
import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedSpeakerLow;
import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndFerry;
import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndFerryManual;
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.Ports;
import com.stuypulse.robot.constants.Settings;
Expand Down Expand Up @@ -101,59 +107,82 @@ private void configureDefaultCommands() {
/***************/

private void configureButtonBindings() {
driver.getRightBumper().whileTrue(new SwerveDriveXMode());
configureOperatorBindings();
configureDriverBindings();
}

driver.getLeftMenuButton().onTrue(new SwerveDriveSeedFieldRelative());
driver.getRightMenuButton().onTrue(new SwerveDriveAutoAlignment(driver));
private void configureDriverBindings() {
driver.getRightMenuButton().onTrue(new SwerveDriveSeedFieldRelative());

driver.getLeftTriggerButton()
// intake field relative
driver.getRightTriggerButton()
.onTrue(new ArmToFeed())
.whileTrue(new SwerveDriveDriveToNote(driver))
.whileTrue(new IntakeAcquire()
.andThen(new BuzzController(driver))
);

driver.getLeftBumper()
// intake robot relative
driver.getLeftTriggerButton()
.onTrue(new ArmToFeed())
.whileTrue(new IntakeAcquire()
.andThen(new BuzzController(driver))
)
.whileTrue(new SwerveDriveDriveRobotRelative(driver));

// deacquire
driver.getDPadLeft()
.whileTrue(new IntakeDeacquire())
.onFalse(new IntakeStop());

driver.getRightTriggerButton()
.whileTrue(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
.andThen(new ShooterAutoShoot())
)
.onFalse(new ShooterFeederStop())
.onFalse(new ShooterSetRPM(new ShooterSpeeds()).onlyIf(() -> !Settings.Shooter.ALWAYS_KEEP_AT_SPEED));

driver.getTopButton()
.onTrue(new ArmToSpeaker())
.onTrue(new BuzzController(driver).onlyIf(() -> !Shooter.getInstance().hasNote()));
driver.getDPadUp().onTrue(new ArmSetShootHeightToHigh());
driver.getDPadDown().onTrue(new ArmSetShootHeightToLow());

driver.getTopButton()
.debounce(Settings.Driver.HOLD_TO_OVERRIDE_TIME)
.onTrue(new ArmEnableOverride())
.onFalse(new ArmDisableOverride());
// speaker align and score
driver.getRightBumper()
.whileTrue(new SwerveDriveDriveAndScoreSpeaker(driver));

driver.getLeftButton()
.onTrue(new ArmToAmp())
.onTrue(new BuzzController(driver).onlyIf(() -> !Shooter.getInstance().hasNote()));

driver.getLeftButton()
.debounce(Settings.Driver.HOLD_TO_OVERRIDE_TIME)
.onTrue(new ArmEnableOverride())
.onFalse(new ArmDisableOverride());
// ferry align and shoot
// move to back of controller
driver.getRightStickButton()
.whileTrue(new SwerveDriveDriveAndFerry(driver));

// arm to amp and alignment
driver.getLeftBumper()
.onTrue(new ArmToAmp())
.onTrue(new SwerveDriveDriveAlignedAmp(driver));

// manual speaker at subwoofer
// score amp
// rebind to a button on the back later
driver.getLeftStickButton()
.whileTrue(new ConditionalCommand(
new ShooterScoreAmp(),
new ArmToSubwooferShot()
.andThen(new ShooterScoreSpeaker()),
() -> Arm.getInstance().getState() == Arm.State.AMP))
.onFalse(new ConditionalCommand(
new ShooterFeederStop(),
new ShooterStop(),
() -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));

// manual ferry
driver.getTopButton()
.whileTrue(new SwerveDriveDriveAndFerryManual(driver));

driver.getRightButton()
.whileTrue(new ArmToFerry())
.onTrue(new BuzzController(driver).onlyIf(() -> !Shooter.getInstance().hasNote()));

.onTrue(new ArmToPreClimb());
driver.getRightButton()
.debounce(Settings.Driver.HOLD_TO_OVERRIDE_TIME)
.onTrue(new ArmEnableOverride())
.onFalse(new ArmDisableOverride());

driver.getBottomButton().onTrue(new ArmToFeed());
.debounce(Settings.Driver.TIME_UNTIL_HOLD)
.whileTrue(new SwerveDriveDriveToChain());

driver.getDPadUp().whileTrue(new ArmToPreClimb());
driver.getDPadDown().whileTrue(new ArmToStow());
driver.getBottomButton().whileTrue(new SwerveDriveDriveToChain());
driver.getLeftMenuButton().onTrue(new ArmToClimbing());
}

private void configureOperatorBindings() {
operator.getLeftTriggerButton().whileTrue(new IntakeDeacquire());
operator.getRightTriggerButton().whileTrue(new IntakeAcquire());
}

/**************/
Expand Down
Original file line number Diff line number Diff line change
@@ -1,19 +1,19 @@
package com.stuypulse.robot.commands.arm;

import com.stuypulse.robot.subsystems.arm.Arm;

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

public class ArmEnableOverride extends InstantCommand{
public class ArmSetShootHeightToHigh extends InstantCommand{

private final Arm arm;

public ArmEnableOverride() {
this.arm = Arm.getInstance();
public ArmSetShootHeightToHigh() {
arm = Arm.getInstance();
addRequirements(arm);
}

@Override
public void initialize() {
arm.setOverriding(true);
arm.setShootHeightHigh();
}
}
Original file line number Diff line number Diff line change
@@ -1,19 +1,19 @@
package com.stuypulse.robot.commands.arm;

import com.stuypulse.robot.subsystems.arm.Arm;

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

public class ArmDisableOverride extends InstantCommand{
public class ArmSetShootHeightToLow extends InstantCommand{

private final Arm arm;

public ArmDisableOverride() {
this.arm = Arm.getInstance();
public ArmSetShootHeightToLow() {
arm = Arm.getInstance();
addRequirements(arm);
}

@Override
public void initialize() {
arm.setOverriding(false);
arm.setShootHeightLow();
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,6 @@ public ArmSetState(Arm.State state) {

@Override
public void initialize() {
arm.setRequestedState(state);
arm.setState(state);
}
}
10 changes: 10 additions & 0 deletions src/main/java/com/stuypulse/robot/commands/arm/ArmToClimbing.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
package com.stuypulse.robot.commands.arm;

import com.stuypulse.robot.subsystems.arm.Arm;

public class ArmToClimbing extends ArmSetState{

public ArmToClimbing(){
super(Arm.State.CLIMBING);
}
}
31 changes: 4 additions & 27 deletions src/main/java/com/stuypulse/robot/commands/arm/ArmToFerry.java
Original file line number Diff line number Diff line change
@@ -1,33 +1,10 @@
package com.stuypulse.robot.commands.arm;

import com.stuypulse.robot.constants.Settings;
import com.stuypulse.robot.subsystems.arm.Arm;
import com.stuypulse.stuylib.util.StopWatch;

import edu.wpi.first.wpilibj2.command.InstantCommand;
public class ArmToFerry extends ArmSetState{

public class ArmToFerry extends InstantCommand{

private final Arm arm;
private final StopWatch stopWatch;
private double lastClick;

public ArmToFerry() {
arm = Arm.getInstance();
stopWatch = new StopWatch();
lastClick = 0;
addRequirements(arm);
}

@Override
public void initialize() {
if (stopWatch.getTime() - lastClick < Settings.Driver.DOUBLE_CLICK_TIME_BETWEEN_CLICKS) {
arm.setRequestedState(Arm.State.LOB_FERRY);
}
else {
arm.setRequestedState(Arm.State.LOW_FERRY);
}
lastClick = stopWatch.getTime();
public ArmToFerry(){
super(Arm.State.SPEAKER);
}

}
}
10 changes: 0 additions & 10 deletions src/main/java/com/stuypulse/robot/commands/arm/ArmToLobFerry.java

This file was deleted.

10 changes: 0 additions & 10 deletions src/main/java/com/stuypulse/robot/commands/arm/ArmToLowFerry.java

This file was deleted.

31 changes: 4 additions & 27 deletions src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeaker.java
Original file line number Diff line number Diff line change
@@ -1,33 +1,10 @@
package com.stuypulse.robot.commands.arm;

import com.stuypulse.robot.constants.Settings;
import com.stuypulse.robot.subsystems.arm.Arm;
import com.stuypulse.stuylib.util.StopWatch;

import edu.wpi.first.wpilibj2.command.InstantCommand;
public class ArmToSpeaker extends ArmSetState{

public class ArmToSpeaker extends InstantCommand{

private final Arm arm;
private final StopWatch stopWatch;
private double lastClick;

public ArmToSpeaker() {
arm = Arm.getInstance();
stopWatch = new StopWatch();
lastClick = 0;
addRequirements(arm);
}

@Override
public void initialize() {
if (stopWatch.getTime() - lastClick < Settings.Driver.DOUBLE_CLICK_TIME_BETWEEN_CLICKS) {
arm.setRequestedState(Arm.State.SPEAKER_HIGH);
}
else {
arm.setRequestedState(Arm.State.SPEAKER_LOW);
}
lastClick = stopWatch.getTime();
public ArmToSpeaker(){
super(Arm.State.SPEAKER);
}

}
}

This file was deleted.

This file was deleted.

Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
package com.stuypulse.robot.commands.arm;

import com.stuypulse.robot.subsystems.arm.Arm;

public class ArmToSubwooferShot extends ArmSetState{

public ArmToSubwooferShot(){
super(Arm.State.SUBWOOFER_SHOT);
}
}
Loading

0 comments on commit acd3e05

Please sign in to comment.