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

8/18 #31

Merged
merged 4 commits into from
Aug 22, 2024
Merged

8/18 #31

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
f3e49d6
separate intakeShoot angles and feed angles
IanShiii Aug 18, 2024
2110682
tune handoff
IanShiii Aug 18, 2024
8ac0a2b
state machine for feeder, automatically retract note into shooter bef…
IanShiii Aug 22, 2024
1d02dd0
remove manual intake feeder button
IanShiii Aug 22, 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
16 changes: 14 additions & 2 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 @@ -3,6 +3,7 @@
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.shooter.ShooterManualIntake;
import com.stuypulse.robot.commands.vision.VisionReloadWhiteList;
import com.stuypulse.robot.constants.Settings;
import com.stuypulse.robot.constants.Settings.RobotType;
Expand Down Expand Up @@ -51,9 +52,20 @@ public void robotPeriodic() {
}

if (Arm.getInstance().getVelocity() > Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED
&& Arm.getInstance().atValidFeedAngle()
&& Arm.getInstance().atIntakeShouldShootAngle()
) {
CommandScheduler.getInstance().schedule(new IntakeShoot().until(() -> Arm.getInstance().getVelocity() < Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED));
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
18 changes: 8 additions & 10 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 @@ -162,7 +162,7 @@ private void configureDriverBindings() {
.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().atValidFeedAngle()))
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
)
)
.alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)
Expand All @@ -176,14 +176,14 @@ private void configureDriverBindings() {
() -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));

// lob ferry align and shoot
driver.getDPadRight()
driver.getLeftStickButton()
.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()
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atValidFeedAngle()))
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
)
)
.alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN)
Expand All @@ -198,14 +198,14 @@ private void configureDriverBindings() {


// low ferry align and shoot
driver.getDPadDown()
driver.getRightStickButton()
.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()
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atValidFeedAngle()))
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
)
)
.alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN)
Expand All @@ -227,7 +227,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 ShooterFeederShoot()
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atValidFeedAngle()))
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
)
)
.whileTrue(new LEDSet(LEDInstructions.SPEAKER_MANUAL))
Expand All @@ -244,7 +244,7 @@ private void configureDriverBindings() {
.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().atValidFeedAngle()))
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
)
)
.alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN_MANUAL)
Expand All @@ -265,7 +265,7 @@ private void configureDriverBindings() {
.alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)))
.andThen(new WaitUntilCommand(() -> swerve.isAlignedToManualLowFerry()))
.andThen(new ShooterFeederShoot()
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atValidFeedAngle()))
.alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
)
)
.alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN_MANUAL)
Expand All @@ -281,8 +281,6 @@ 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/shooter/ShooterAcquireFromIntake.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ public ShooterAcquireFromIntake() {
public void initialize() {
stopWatch.reset();
intake.setState(Intake.State.FEEDING);
shooter.feederIntake();
shooter.setFeederState(Shooter.FeederState.INTAKING);
}

@Override
Expand Down Expand Up @@ -57,7 +57,7 @@ public boolean isFinished() {

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

Expand Down
2 changes: 1 addition & 1 deletion src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederAcquire.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ public ShooterFeederAcquire() {

@Override
public void initialize() {
shooter.feederIntake();
shooter.setFeederState(Shooter.FeederState.INTAKING);
}

}
2 changes: 1 addition & 1 deletion src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederDeacquire.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,6 @@ public ShooterFeederDeacquire() {

@Override
public void initialize() {
shooter.feederDeacquire();
shooter.setFeederState(Shooter.FeederState.DEACQUIRING);
}
}
2 changes: 1 addition & 1 deletion src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederShoot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,6 @@ public ShooterFeederShoot() {

@Override
public void initialize() {
shooter.feederShoot();
shooter.setFeederState(Shooter.FeederState.SHOOTING);
}
}
2 changes: 1 addition & 1 deletion src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederStop.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ public ShooterFeederStop() {

@Override
public void initialize() {
shooter.feederStop();
shooter.setFeederState(Shooter.FeederState.STOP);
}

}
9 changes: 7 additions & 2 deletions src/main/java/com/stuypulse/robot/commands/shooter/ShooterManualIntake.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,11 +15,16 @@ public ShooterManualIntake() {

@Override
public void initialize() {
shooter.feederIntake();
shooter.setFeederState(Shooter.FeederState.INTAKING);
}

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

@Override
public void end(boolean interrupted) {
shooter.feederStop();
shooter.setFeederState(Shooter.FeederState.STOP);
}
}
19 changes: 11 additions & 8 deletions src/main/java/com/stuypulse/robot/constants/Settings.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,8 +58,8 @@ 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 : 1000);
SmartNumber MAX_ACCELERATION = new SmartNumber("Arm/Max Acceleration (deg/s^2)", SAFE_MODE_ENABLED ? 200 : 800);
SmartNumber MAX_VELOCITY = new SmartNumber("Arm/Max Velocity (deg/s)", SAFE_MODE_ENABLED ? 200 : 900);
SmartNumber MAX_ACCELERATION = new SmartNumber("Arm/Max Acceleration (deg/s^2)", SAFE_MODE_ENABLED ? 200 : 700);

SmartNumber MAX_ANGLE = new SmartNumber("Arm/Max Angle (deg)", 85);
SmartNumber MIN_ANGLE = new SmartNumber("Arm/Min Angle (deg)", -90 + 12.25);
Expand All @@ -71,8 +71,10 @@ public interface Arm {
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() + 0);
SmartNumber MAX_ACCEPTABLE_FEED_ANGLE = new SmartNumber("Arm/Max Acceptable Feed Angle", FEED_ANGLE.get() + 20);
SmartNumber MAX_ACCEPTABLE_FEED_ANGLE = new SmartNumber("Arm/Max Acceptable Feed Angle", FEED_ANGLE.get() + 4);

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 @@ -100,11 +102,12 @@ public interface Encoder {
}

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

double INTAKE_FEED_SPEED = 0.48;
double INTAKE_FEED_SPEED = 0.4;

double MAX_ARM_ANGLE_FOR_INTAKE_SHOOT = Arm.MIN_ANGLE.get() + 25;
double ARM_SPEED_THRESHOLD_TO_FEED = 2.5; // degrees per second

double INTAKE_SHOOT_SPEED = 0.9;
Expand All @@ -113,14 +116,14 @@ public interface Intake {
double FUNNEL_ACQUIRE = 1.0;
double FUNNEL_DEACQUIRE = 1.0;

double IR_DEBOUNCE = .005;
double IR_DEBOUNCE = 0.0;

double HANDOFF_TIMEOUT = 1.5;
double HANDOFF_TIMEOUT = 1.0;
double MINIMUM_DEACQUIRE_TIME_WHEN_STUCK = 0.5;
}

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

Expand Down
1 change: 1 addition & 0 deletions src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@ public State getState() {
public abstract boolean atTarget();

public abstract boolean atValidFeedAngle();
public abstract boolean atIntakeShouldShootAngle();

public abstract double getVelocity();

Expand Down
9 changes: 7 additions & 2 deletions src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,12 @@ public boolean atTarget() {
@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();
&& getDegrees() < Settings.Arm.MAX_ACCEPTABLE_FEED_ANGLE.get() + Settings.Arm.MAX_ANGLE_ERROR.get();
}

@Override
public boolean atIntakeShouldShootAngle() {
return getDegrees() < Settings.Intake.MAX_ARM_ANGLE_FOR_INTAKE_SHOOT;
}

private double getTargetDegrees() {
Expand Down Expand Up @@ -173,7 +178,7 @@ public void periodic() {
}
else {
controller.update(SLMath.clamp(getTargetDegrees(), Settings.Arm.MIN_ANGLE.get(), Settings.Arm.MAX_ANGLE.get()), getDegrees());
if (Shooter.getInstance().isShooting()) {
if (Shooter.getInstance().getFeederState() == Shooter.FeederState.SHOOTING) {
setVoltage(controller.getOutput() + 0.31);
}
else {
Expand Down
27 changes: 19 additions & 8 deletions src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,11 +17,21 @@ public abstract class Shooter extends SubsystemBase {
public static Shooter getInstance() {
return instance;
}

public enum FeederState {
INTAKING,
DEACQUIRING,
SHOOTING,
STOP
}

private FeederState feederState;

private final SmartNumber leftTargetRPM;
private final SmartNumber rightTargetRPM;

public Shooter() {
feederState = FeederState.STOP;
leftTargetRPM = new SmartNumber("Shooter/Left Target RPM", Settings.Shooter.ALWAYS_KEEP_AT_SPEED ? Settings.Shooter.SPEAKER.getLeftRPM() : 0);
rightTargetRPM = new SmartNumber("Shooter/Right Target RPM", Settings.Shooter.ALWAYS_KEEP_AT_SPEED ? Settings.Shooter.SPEAKER.getRightRPM() : 0);
}
Expand All @@ -40,21 +50,22 @@ public double getRightTargetRPM() {
}

public void stop() {
feederStop();
setFeederState(FeederState.STOP);
leftTargetRPM.set(0);
rightTargetRPM.set(0);
}

public abstract boolean atTargetSpeeds();
public void setFeederState(FeederState feederState) {
this.feederState = feederState;
}

public abstract ShooterSpeeds getFerrySpeeds();
public FeederState getFeederState() {
return feederState;
}

public abstract void feederIntake();
public abstract void feederDeacquire();
public abstract void feederShoot();
public abstract void feederStop();
public abstract boolean atTargetSpeeds();

public abstract boolean isShooting();
public abstract ShooterSpeeds getFerrySpeeds();

public abstract boolean hasNote();
}
47 changes: 20 additions & 27 deletions src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -106,33 +106,24 @@ private void setRightShooterRPM(double rpm) {
rightController.setReference(rpm, ControlType.kVelocity);
}

@Override
public void feederIntake() {
feederMotor.set(+Settings.Shooter.FEEDER_INTAKE_SPEED);
isShooting = false;
}

@Override
public void feederDeacquire() {
feederMotor.set(-Settings.Shooter.FEEDER_DEAQUIRE_SPEED);
isShooting = false;
}

@Override
public void feederShoot() {
feederMotor.set(Settings.Shooter.FEEDER_SHOOT_SPEED);
isShooting = true;
}

@Override
public void feederStop() {
feederMotor.set(0);
isShooting = false;
}

@Override
public boolean isShooting() {
return isShooting;
private void setFeederBasedOnState() {
switch (getFeederState()) {
case INTAKING:
feederMotor.set(+Settings.Shooter.FEEDER_INTAKE_SPEED);
break;
case DEACQUIRING:
feederMotor.set(-Settings.Shooter.FEEDER_DEAQUIRE_SPEED);
break;
case SHOOTING:
feederMotor.set(Settings.Shooter.FEEDER_SHOOT_SPEED);
break;
case STOP:
feederMotor.set(0);
break;
default:
feederMotor.set(0);
break;
}
}

@Override
Expand Down Expand Up @@ -176,6 +167,8 @@ public void periodic () {
setRightShooterRPM(getRightTargetRPM());
}

setFeederBasedOnState();

SmartDashboard.putNumber("Shooter/Feeder Speed", feederMotor.get());

SmartDashboard.putNumber("Shooter/Left Voltage", leftMotor.getBusVoltage());
Expand Down
Loading