Skip to content

Commit

Permalink
Merge pull request #30 from StuyPulse/8-14
Browse files Browse the repository at this point in the history
  • Loading branch information
@IanShiii
IanShiii authored Aug 15, 2024
2 parents 5aa1fe5 + 999729e commit 9113259
Show file tree
Hide file tree
Showing 10 changed files with 139 additions and 36 deletions.
8 changes: 8 additions & 0 deletions src/main/java/com/stuypulse/robot/Robot.java
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,10 @@
package com.stuypulse.robot;

import com.stuypulse.robot.commands.BuzzController;
import com.stuypulse.robot.commands.intake.IntakeShoot;
import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntake;
import com.stuypulse.robot.commands.vision.VisionReloadWhiteList;
import com.stuypulse.robot.constants.Settings;
import com.stuypulse.robot.constants.Settings.RobotType;
import com.stuypulse.robot.subsystems.arm.Arm;
import com.stuypulse.robot.subsystems.intake.Intake;
Expand Down Expand Up @@ -47,6 +49,12 @@ public void robotPeriodic() {
CommandScheduler.getInstance().schedule(new ShooterAcquireFromIntake()
.andThen(new BuzzController(robot.driver)));
}

if (Arm.getInstance().getVelocity() > Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED
&& Arm.getInstance().atValidFeedAngle()
) {
CommandScheduler.getInstance().schedule(new IntakeShoot());
}

CommandScheduler.getInstance().run();
}
Expand Down
58 changes: 36 additions & 22 deletions src/main/java/com/stuypulse/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,10 @@
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.IntakeAcquireForever;
import com.stuypulse.robot.commands.intake.IntakeDeacquire;
import com.stuypulse.robot.commands.intake.IntakeFeed;
import com.stuypulse.robot.commands.intake.IntakeShoot;
import com.stuypulse.robot.commands.intake.IntakeStop;
import com.stuypulse.robot.commands.leds.LEDDefaultMode;
import com.stuypulse.robot.commands.leds.LEDReset;
Expand All @@ -22,6 +25,7 @@
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.ShooterManualIntake;
import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker;
import com.stuypulse.robot.commands.shooter.ShooterSetRPM;
import com.stuypulse.robot.commands.shooter.ShooterStop;
Expand Down Expand Up @@ -106,6 +110,7 @@ public RobotContainer() {

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

Expand All @@ -124,7 +129,7 @@ private void configureDriverBindings() {
// intake field relative
driver.getRightTriggerButton()
.onTrue(new ArmToFeed())
.whileTrue(new SwerveDriveDriveToNote(driver))
// .whileTrue(new SwerveDriveDriveToNote(driver))
.whileTrue(new IntakeAcquire()
.deadlineWith(new LEDSet(LEDInstructions.INTAKING))
.andThen(new BuzzController(driver))
Expand All @@ -141,9 +146,8 @@ private void configureDriverBindings() {

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

// speaker align and score
// score amp
Expand All @@ -157,7 +161,9 @@ 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.isAlignedToSpeaker()))
.andThen(new ShooterFeederShoot())
.andThen(new ShooterFeederShoot()
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atValidFeedAngle()))
)
)
.alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)
.until(() -> swerve.isAlignedToSpeaker())
Expand All @@ -169,15 +175,16 @@ private void configureDriverBindings() {
new ShooterStop(),
() -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));

// ferry align and shoot
// move to back of controller
// lob ferry align and shoot
driver.getDPadRight()
.whileTrue(new SwerveDriveDriveAlignedLobFerry(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.isAlignedToLobFerry()))
.andThen(new ShooterFeederShoot())
.andThen(new ShooterFeederShoot()
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atValidFeedAngle()))
)
)
.alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN)
.until(() -> swerve.isAlignedToLobFerry())
Expand All @@ -189,13 +196,17 @@ private void configureDriverBindings() {
new ShooterStop(),
() -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));


// low ferry align and shoot
driver.getDPadDown()
.whileTrue(new SwerveDriveDriveAlignedLowFerry(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.isAlignedToLowFerry()))
.andThen(new ShooterFeederShoot())
.andThen(new ShooterFeederShoot()
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atValidFeedAngle()))
)
)
.alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN)
.until(() -> swerve.isAlignedToLowFerry())
Expand All @@ -207,36 +218,34 @@ private void configureDriverBindings() {
new ShooterStop(),
() -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));

// arm to amp and alignment
driver.getLeftBumper()
.onTrue(new ArmToAmp())
.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)));
// arm to amp
driver.getLeftBumper().onTrue(new ArmToAmp());

// manual speaker at subwoofer
// rebind to a button on the back later
driver.getRightMenuButton()
.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()))
.andThen(new ShooterFeederShoot()
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atValidFeedAngle()))
)
)
.whileTrue(new LEDSet(LEDInstructions.SPEAKER_MANUAL))
.onFalse(new ConditionalCommand(
new ShooterFeederStop(),
new ShooterStop(),
() -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));

// manual ferry
// manual lob ferry
driver.getTopButton()
.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())
.andThen(new ShooterFeederShoot()
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atValidFeedAngle()))
)
)
.alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN_MANUAL)
.until(() -> swerve.isAlignedToManualLobFerry())
Expand All @@ -248,13 +257,16 @@ private void configureDriverBindings() {
new ShooterStop(),
() -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));

// manual low ferry
driver.getLeftButton()
.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())
.andThen(new ShooterFeederShoot()
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atValidFeedAngle()))
)
)
.alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN_MANUAL)
.until(() -> swerve.isAlignedToManualLowFerry())
Expand All @@ -269,6 +281,8 @@ private void configureDriverBindings() {
// climbing
driver.getRightButton().onTrue(new ArmToPreClimb());
driver.getBottomButton().onTrue(new ArmToClimbing());

driver.getLeftMenuButton().whileTrue(new ShooterManualIntake());
}

private void configureOperatorBindings() {
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,9 @@

import com.stuypulse.robot.subsystems.intake.Intake;

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

public class IntakeDeacquire extends InstantCommand {
public class IntakeDeacquire extends Command {

private final Intake intake;

Expand Down
19 changes: 19 additions & 0 deletions src/main/java/com/stuypulse/robot/commands/intake/IntakeShoot.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
package com.stuypulse.robot.commands.intake;

import com.stuypulse.robot.constants.Settings;
import com.stuypulse.robot.subsystems.intake.Intake;

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

public class IntakeShoot extends SequentialCommandGroup {

public IntakeShoot() {
addCommands(
new InstantCommand(() -> Intake.getInstance().setState(Intake.State.SHOOTING), Intake.getInstance()),
new WaitCommand(Settings.Intake.INTAKE_SHOOT_TIME),
new IntakeStop()
);
}
}
25 changes: 25 additions & 0 deletions src/main/java/com/stuypulse/robot/commands/shooter/ShooterManualIntake.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
package com.stuypulse.robot.commands.shooter;

import com.stuypulse.robot.subsystems.shooter.Shooter;

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

public class ShooterManualIntake extends Command{

private final Shooter shooter;

public ShooterManualIntake() {
shooter = Shooter.getInstance();
addRequirements(shooter);
}

@Override
public void initialize() {
shooter.feederIntake();
}

@Override
public void end(boolean interrupted) {
shooter.feederStop();
}
}
28 changes: 17 additions & 11 deletions src/main/java/com/stuypulse/robot/constants/Settings.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,20 +58,21 @@ public static RobotType fromString(String serialNum) {
public interface Arm {
double LENGTH = Units.inchesToMeters(16.5);

SmartNumber MAX_VELOCITY = new SmartNumber("Arm/Max Velocity (deg/s)", SAFE_MODE_ENABLED ? 200 : 350);
SmartNumber MAX_ACCELERATION = new SmartNumber("Arm/Max Acceleration (deg/s^2)", SAFE_MODE_ENABLED ? 200 : 350);
SmartNumber MAX_VELOCITY = new SmartNumber("Arm/Max Velocity (deg/s)", SAFE_MODE_ENABLED ? 200 : 1000);
SmartNumber MAX_ACCELERATION = new SmartNumber("Arm/Max Acceleration (deg/s^2)", SAFE_MODE_ENABLED ? 200 : 800);

SmartNumber MAX_ANGLE = new SmartNumber("Arm/Max Angle (deg)", 85);
SmartNumber MIN_ANGLE = new SmartNumber("Arm/Min Angle (deg)", -90 + 12.25);

SmartNumber MAX_ANGLE_ERROR = new SmartNumber("Arm/Max Angle Error", 2.5);

SmartNumber AMP_ANGLE = new SmartNumber("Arm/Amp Angle", 50);
SmartNumber AMP_ANGLE = new SmartNumber("Arm/Amp Angle", 40);
SmartNumber LOW_FERRY_ANGLE = new SmartNumber("Arm/Low Ferry Angle", -50);
SmartNumber LOB_FERRY_ANGLE = new SmartNumber("Arm/Lob Ferry Angle", 50);
SmartNumber PRE_CLIMB_ANGLE = new SmartNumber("Arm/Pre climb angle", 80);
SmartNumber POST_CLIMB_ANGLE = new SmartNumber("Arm/Post Climb Angle", MIN_ANGLE.get() + 7);
SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.get() + 17);
SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.get() + 0);
SmartNumber MAX_ACCEPTABLE_FEED_ANGLE = new SmartNumber("Arm/Max Acceptable Feed Angle", FEED_ANGLE.get() + 20);
SmartNumber SUBWOOFER_SHOT_ANGLE = new SmartNumber("Arm/Subwoofer Shot Angle", -33);

SmartNumber BUMP_SWITCH_DEBOUNCE_TIME = new SmartNumber("Arm/Bump Switch Debounce Time", 0.02);
Expand Down Expand Up @@ -99,10 +100,15 @@ public interface Encoder {
}

public interface Intake {
double INTAKE_ACQUIRE_SPEED = 0.65;
double INTAKE_ACQUIRE_SPEED = 1.0;
double INTAKE_DEACQUIRE_SPEED = 1.0;

double INTAKE_FEED_SPEED = 0.65;
double INTAKE_FEED_SPEED = 0.48;

double ARM_SPEED_THRESHOLD_TO_FEED = 2.5; // degrees per second

double INTAKE_SHOOT_SPEED = 0.9;
double INTAKE_SHOOT_TIME = 0.75;

double FUNNEL_ACQUIRE = 1.0;
double FUNNEL_DEACQUIRE = 1.0;
Expand All @@ -114,7 +120,7 @@ public interface Intake {
}

public interface Shooter {
double FEEDER_INTAKE_SPEED = 0.23;
double FEEDER_INTAKE_SPEED = 0.19;
double FEEDER_DEAQUIRE_SPEED = 0.5;
double FEEDER_SHOOT_SPEED = 1.0;

Expand All @@ -129,8 +135,8 @@ public interface Shooter {
);

// Different falling debounce is used to detect note shooting;
SmartNumber HAS_NOTE_FALLING_DEBOUNCE = new SmartNumber("Shooter/Has Note Falling Debounce", 0.01);
SmartNumber HAS_NOTE_RISING_DEBOUNCE = new SmartNumber("Shooter/Has Note Rising Debounce", 0.005);
SmartNumber HAS_NOTE_FALLING_DEBOUNCE = new SmartNumber("Shooter/Has Note Falling Debounce", 0.0); //0.01
SmartNumber HAS_NOTE_RISING_DEBOUNCE = new SmartNumber("Shooter/Has Note Rising Debounce", 0.0); //0.005

// left runs faster than right
public interface LEFT {
Expand Down Expand Up @@ -404,8 +410,8 @@ public interface Vision {
}

public interface Buzz {
double BUZZ_DURATION = 0.2;
double BUZZ_INTENSITY = 1;
double BUZZ_DURATION = 1.0;
double BUZZ_INTENSITY = 1.0;
}

public interface Auton {
Expand Down
4 changes: 4 additions & 0 deletions src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,10 @@ public State getState() {

public abstract boolean atTarget();

public abstract boolean atValidFeedAngle();

public abstract double getVelocity();

@Override
public void periodic() {
SmartDashboard.putString("Arm/State", state.toString());
Expand Down
13 changes: 12 additions & 1 deletion src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,11 +69,17 @@ protected ArmImpl() {
@Override
public boolean atTarget() {
if (state == State.FEED) {
return getDegrees() < Settings.Arm.FEED_ANGLE.get() + Settings.Arm.MAX_ANGLE_ERROR.get();
return atValidFeedAngle();
}
return Math.abs(getTargetDegrees() - getDegrees()) < Settings.Arm.MAX_ANGLE_ERROR.get();
}

@Override
public boolean atValidFeedAngle() {
return getDegrees() > Settings.Arm.FEED_ANGLE.get() - Settings.Arm.MAX_ANGLE_ERROR.get()
|| getDegrees() < Settings.Arm.MAX_ACCEPTABLE_FEED_ANGLE.get() + Settings.Arm.MAX_ANGLE_ERROR.get();
}

private double getTargetDegrees() {
switch (state) {
case AMP:
Expand Down Expand Up @@ -140,6 +146,11 @@ private double getDegrees() {
return 360 * armEncoder.getPosition();
}

@Override
public double getVelocity() {
return 360 / 60 * armEncoder.getVelocity();
}

private void setVoltage(double voltage) {
leftMotor.setVoltage(voltage);
rightMotor.setVoltage(voltage);
Expand Down
7 changes: 7 additions & 0 deletions src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
package com.stuypulse.robot.subsystems.intake;

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;


Expand All @@ -19,6 +20,7 @@ public enum State {
DEACQUIRING,
ACQUIRING,
FEEDING,
SHOOTING,
STOP
}

Expand All @@ -37,4 +39,9 @@ public State getState() {
}

public abstract boolean hasNote();

@Override
public void periodic() {
SmartDashboard.putString("Intake/State", state.name());
}
}
Loading

0 comments on commit 9113259

Please sign in to comment.