Skip to content

Commit

Permalink
8/28 (#37) (#38)
Browse files Browse the repository at this point in the history 
* fix camera rotations

* get rid of outlier data

* move automatically scheduled commands to triggers inside robot container

* potential fix for having to double click to shoot

* feeder runs backwards on deacquire, dont retry intake

* tune amp angle, fix intake led colors

* switch ferry buttons

* added arm climb commands

* clean up climb commands and default led instruction

* use old low ferry data as lob ferry data, fix arm angles for ferrying

* sysid swerve

---------

Co-authored-by: Tmanxyz <[email protected]>
  • Loading branch information
@IanShiii @Tmanxyz
IanShiii and Tmanxyz authored Sep 2, 2024
1 parent 08da479 commit 9814c18
Show file tree
Hide file tree
Showing 10 changed files with 182 additions and 221 deletions.
27 changes: 0 additions & 27 deletions src/main/java/com/stuypulse/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,33 +57,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
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,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 @@ -71,6 +72,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 @@ -124,10 +126,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 @@ -150,23 +160,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 @@ -182,9 +191,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 @@ -201,9 +208,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 @@ -220,9 +225,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 @@ -237,9 +240,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 @@ -274,6 +275,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
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
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
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
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
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
14 changes: 7 additions & 7 deletions src/main/java/com/stuypulse/robot/constants/Settings.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,11 +66,11 @@ public interface Arm {

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

SmartNumber AMP_ANGLE = new SmartNumber("Arm/Amp Angle", 43);
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 AMP_ANGLE = new SmartNumber("Arm/Amp Angle", 49);
SmartNumber LOW_FERRY_ANGLE = new SmartNumber("Arm/Low Ferry Angle", MIN_ANGLE.get());
SmartNumber LOB_FERRY_ANGLE = new SmartNumber("Arm/Lob Ferry Angle", -50);
SmartNumber PRE_CLIMB_ANGLE = new SmartNumber("Arm/Pre climb angle", 90);
SmartNumber POST_CLIMB_ANGLE = new SmartNumber("Arm/Post Climb Angle", MIN_ANGLE.get());

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() + 4);
Expand Down Expand Up @@ -108,7 +108,7 @@ public interface Intake {
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 ARM_SPEED_THRESHOLD_TO_FEED = 1.75; // degrees per second

double INTAKE_SHOOT_SPEED = 0.9;
double INTAKE_SHOOT_TIME = 0.75;
Expand All @@ -127,7 +127,7 @@ public interface Shooter {
double FEEDER_DEAQUIRE_SPEED = 0.5;
double FEEDER_SHOOT_SPEED = 1.0;

boolean ALWAYS_KEEP_AT_SPEED = false;
boolean ALWAYS_KEEP_AT_SPEED = true;

double TARGET_RPM_THRESHOLD = 200;
double MAX_WAIT_TO_REACH_TARGET = ALWAYS_KEEP_AT_SPEED ? 1.5 : 2.0;
Expand Down
Loading

0 comments on commit 9814c18

Please sign in to comment.