Skip to content

Commit

Permalink
put reset to sub on leftstick, make shootwhenready allow for green fn…
Browse files Browse the repository at this point in the history 
… if no hdc aiming is happening, and added some sample interpolation maps to try out if shooter speeds are dog
  • Loading branch information
@Oliver-Cushman
Oliver-Cushman committed Oct 6, 2024
1 parent ee600da commit 4f16afc
Show file tree
Hide file tree
Showing 4 changed files with 107 additions and 32 deletions.
73 changes: 47 additions & 26 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -382,6 +382,7 @@ private void configureTimedEvents() {

new Trigger(climb::getToggleMode)
.onTrue(swerve.resetHDCCommand())
.onFalse(swerve.resetDesiredHDCPoseCommand())
.whileTrue(alignmentCmds.chainRotationalAlignment(driver::getLeftX, driver::getLeftY, robotRelativeSupplier));
}

Expand Down Expand Up @@ -442,10 +443,10 @@ private void configureCommonDriverBindings(PatriBoxController controller) {

// Speaker / Source / Chain rotational alignment
controller.rightStick()
.onTrue(swerve.resetHDCCommand())
.toggleOnTrue(
Commands.sequence(
elevator.toBottomCommand(),
swerve.resetHDCCommand(),
limelight3g.setLEDState(() -> true),
new ActiveConditionalCommand(
// This runs SWD on heading control
Expand All @@ -455,12 +456,13 @@ private void configureCommonDriverBindings(PatriBoxController controller) {
() -> PoseCalculations.closeToSpeaker() || climb.getHooksUp())
).finallyDo(
() ->
limelight3g.setLEDState(() -> false)
.andThen(
shooterCmds.raisePivot()
.alongWith(shooterCmds.stopShooter())
.withInterruptBehavior(InterruptionBehavior.kCancelSelf))
.schedule()));
Commands.parallel(
swerve.resetDesiredHDCPoseCommand(),
limelight3g.setLEDState(() -> false),
shooterCmds.raisePivot()
.alongWith(shooterCmds.stopShooter())
.withInterruptBehavior(InterruptionBehavior.kCancelSelf))
.schedule()));


// Climbing controls
Expand All @@ -473,8 +475,13 @@ private void configureCommonDriverBindings(PatriBoxController controller) {
// Note to target will either place amp or shoot,
// depending on if the elevator is up or not
controller.rightTrigger()
.onTrue(pieceControl.noteToTarget(swerve::getPose, swerve::getRobotRelativeVelocity, swerve::atHDCAngle, () -> operator.getLeftBumper())
.alongWith(driver.setRumble(() -> 0.5, 0.3))
.onTrue(
pieceControl.noteToTarget(
swerve::getPose,
swerve::getRobotRelativeVelocity,
() -> swerve.atHDCAngle() || swerve.getDesiredPose().equals(new Pose2d()),
() -> operator.getLeftBumper())
.alongWith(driver.setRumble(() -> 0.5, 0.3))
.andThen(Commands.runOnce(() -> RobotContainer.hasPiece = false)));


Expand All @@ -494,19 +501,20 @@ private void configureCommonDriverBindings(PatriBoxController controller) {
controller.povLeft()
.onTrue(pieceControl.stopAllMotors().andThen(shooterCmds.raisePivot()));

controller.povRight()
.toggleOnTrue(Commands.sequence(
elevator.toBottomCommand(),
swerve.resetHDCCommand(),
limelight3g.setLEDState(() -> true),
alignmentCmds.moveAndPreparePresetCommand()
.finallyDo(
() ->
limelight3g.setLEDState(() -> false)
.andThen(shooterCmds.raisePivot()
.alongWith(shooterCmds.stopShooter())
.withInterruptBehavior(InterruptionBehavior.kCancelSelf))
.schedule())));
// Cat smirk
// controller.povRight()
// .toggleOnTrue(Commands.sequence(
// elevator.toBottomCommand(),
// swerve.resetHDCCommand(),
// limelight3g.setLEDState(() -> true),
// alignmentCmds.moveAndPreparePresetCommand()
// .finallyDo(
// () ->
// limelight3g.setLEDState(() -> false)
// .andThen(shooterCmds.raisePivot()
// .alongWith(shooterCmds.stopShooter())
// .withInterruptBehavior(InterruptionBehavior.kCancelSelf))
// .schedule())));

}

Expand All @@ -532,6 +540,7 @@ private void configureSoloDriverBindings(PatriBoxController controller) {
new Trigger(() -> elevator.getDesiredPosition() == ElevatorConstants.TRAP_PLACE_POS
&& swerve.getPose().getY() > FieldConstants.FIELD_HEIGHT_METERS/2.0)
.onTrue(swerve.resetHDCCommand())
.onFalse(swerve.resetDesiredHDCPoseCommand())
.whileTrue(alignmentCmds.ampRotationalAlignmentCommand(driver::getLeftX, driver::getLeftY));

// Subwoofer preset incase something goes south
Expand All @@ -558,7 +567,15 @@ private void configureDriverBindings(PatriBoxController controller) {
configureCommonDriverBindings(controller);

controller.leftStick()
.onTrue(shooterCmds.prepareSubwooferCommand());
.toggleOnTrue(
shooterCmds.prepareSubwooferCommand()
.finallyDo(
() ->
shooterCmds.raisePivot()
.alongWith(shooterCmds.stopShooter())
.withInterruptBehavior(InterruptionBehavior.kCancelSelf)
.schedule()));


controller.a()
.onTrue(swerve.resetHDCCommand())
Expand All @@ -567,7 +584,8 @@ private void configureDriverBindings(PatriBoxController controller) {
.alongWith(
limelight3g.setLEDState(() -> true)))
.onFalse(
limelight3g.setLEDState(() -> false));
limelight3g.setLEDState(() -> false)
.alongWith(swerve.resetHDCCommand()));

controller.x()
.whileTrue(pieceControl.intakeNoteDriver(swerve::getPose, swerve::getRobotRelativeVelocity))
Expand Down Expand Up @@ -611,7 +629,10 @@ private void configureOperatorBindings(PatriBoxController controller) {

controller.rightTrigger()
.onTrue(pieceControl
.noteToTarget(swerve::getPose, swerve::getRobotRelativeVelocity, swerve::atHDCAngle,
.noteToTarget(
swerve::getPose,
swerve::getRobotRelativeVelocity,
() -> swerve.atHDCAngle() || swerve.getDesiredPose().equals(new Pose2d()),
() -> controller.getLeftBumper())
.alongWith(driver.setRumble(() -> 0.5, 0.3))
.andThen(Commands.runOnce(() -> RobotContainer.hasPiece = false)));
Expand Down Expand Up @@ -757,7 +778,7 @@ public void onDisabled() {

public void onEnabled() {
if (Robot.gameMode == GameMode.TELEOP && !DriverStation.isFMSAttached()) {
new LPI(ledStrip, swerve::getPose, driver, swerve::setDesiredPose).schedule();
// new LPI(ledStrip, swerve::getPose, driver, swerve::setDesiredPose).schedule();
} else if (Robot.gameMode == GameMode.TEST) {
CommandScheduler.getInstance().setActiveButtonLoop(testButtonBindingLoop);
}
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/commands/managers/ShooterCmds.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -159,7 +159,7 @@ public Command preparePassCommand(Supplier<Pose2d> robotPose) {
}

public Command prepareSubwooferCommand() {
return setTripletCommand(shooterCalc.calculateSpeakerTriplet(FieldConstants.GET_SUBWOOFER_POSITION().getTranslation()));
return prepareStillSpeakerCommand(FieldConstants::GET_SUBWOOFER_POSITION);
}

public Command getNoteTrajectoryCommand(Supplier<Pose2d> pose, Supplier<ChassisSpeeds> speeds) {
Expand Down
15 changes: 14 additions & 1 deletion src/main/java/frc/robot/subsystems/drive/Swerve.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -271,11 +271,16 @@ public void stopDriving() {
}

@AutoLogOutput (key = "Subsystems/Swerve/DesiredHDCPose")
Pose2d desiredHDCPose = new Pose2d();
private Pose2d desiredHDCPose = new Pose2d();

public void setDesiredPose(Pose2d pose) {
desiredHDCPose = pose;
}

public Pose2d getDesiredPose() {
return desiredHDCPose;
}

public ChassisSpeeds getRobotRelativeVelocity() {
return DriveConstants.DRIVE_KINEMATICS.toChassisSpeeds(getModuleStates());
}
Expand Down Expand Up @@ -454,10 +459,18 @@ public void resetHDC() {
AutoConstants.HDC.getThetaController().reset(getPose().getRotation().getRadians());
}

public void resetDesiredHDCPose() {
setDesiredPose(new Pose2d());
}

public Command resetHDCCommand() {
return Commands.runOnce(() -> resetHDC());
}

public Command resetDesiredHDCPoseCommand() {
return Commands.runOnce(() -> resetDesiredHDCPose());
}

public void reconfigureAutoBuilder() {
AutoBuilder.configureHolonomic(
this::getPose,
Expand Down
49 changes: 45 additions & 4 deletions src/main/java/frc/robot/util/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -210,27 +210,68 @@ public static final class ShooterConstants {
public static final double TUNED_SHOOTER_MAX_DISTANCE = 14.0; // update accordingly with map

/**
* The distances are in feet, the speeds are in RPM, and the angles are in
* degrees.
* The distances are in feet, the speeds are in RPM, and the angles are in
* degrees.
*/
public static final HashMap<Double, SpeedAngleTriplet> SPEAKER_DISTANCES_TO_SPEEDS_AND_ANGLE_MAP = new HashMap<Double, SpeedAngleTriplet>() {
{
put(4.0, SpeedAngleTriplet.of(2316.0, 1763.0, 57.5));
// PRE-BATB
put(4.0, SpeedAngleTriplet.of(2316.0, 1763.0, 56.0));
put(5.0, SpeedAngleTriplet.of(2316.0, 1763.0, 54.1));
put(6.0, SpeedAngleTriplet.of(3007.0, 2818.0, 48.1));
put(7.0, SpeedAngleTriplet.of(3008.0, 2818.0, 44.9));
put(7.5, SpeedAngleTriplet.of(3304.0, 2997.0, 43.3));
put(8.0, SpeedAngleTriplet.of(3319.0, 3024.0, 42.4));
// put(8.5, SpeedAngleTriplet.of(3445.0, 3128.0, 40.9));
put(8.5, SpeedAngleTriplet.of(3445.0, 3128.0, 40.9));
put(9.0, SpeedAngleTriplet.of(3606.0, 3305.0, 39.6));
put(10.0, SpeedAngleTriplet.of(3842.0, 3441.0, 37.6));
put(11.0, SpeedAngleTriplet.of(3965.0, 3647.0, 36.2));
put(12.0, SpeedAngleTriplet.of(4147.0, 3781.0, 34.1));
put(12.5, SpeedAngleTriplet.of(4320.0, 3945.0, 34.0));
put(13.0, SpeedAngleTriplet.of(4490.0, 4106.0, 33.7));
put(14.0, SpeedAngleTriplet.of(4809.0, 4540.0, 33.6));

// PRE-BATB (FLIPPED)
// put(4.0, SpeedAngleTriplet.of(1763.0, 2316.0, 56.0));
// put(5.0, SpeedAngleTriplet.of(1763.0, 2316.0, 54.1));
// put(6.0, SpeedAngleTriplet.of(2818.0, 3007.0, 48.1));
// put(7.0, SpeedAngleTriplet.of(2818.0, 3008.0, 44.9));
// put(7.5, SpeedAngleTriplet.of(2997.0, 3304.0, 43.3));
// put(8.0, SpeedAngleTriplet.of(3024.0, 3319.0, 42.4));
// put(8.5, SpeedAngleTriplet.of(3128.0, 3445.0, 40.9));
// put(9.0, SpeedAngleTriplet.of(3305.0, 3606.0, 39.6));
// put(10.0, SpeedAngleTriplet.of(3441.0, 3842.0, 37.6));
// put(11.0, SpeedAngleTriplet.of(3647.0, 3965.0, 36.2));
// put(12.0, SpeedAngleTriplet.of(3781.0, 4147.0, 34.1));
// put(12.5, SpeedAngleTriplet.of(3945.0, 4320.0, 34.0));
// put(13.0, SpeedAngleTriplet.of(4106.0, 4490.0, 33.7));
// put(14.0, SpeedAngleTriplet.of(4540.0, 4809.0, 33.6));

// BAYOU
// put(6.0, SpeedAngleTriplet.of(3007.0, 2850.0, 50.1));
// put(7.0, SpeedAngleTriplet.of(3160.0, 2865.0, 46.3));
// put(8.0, SpeedAngleTriplet.of(3310.0, 3017.0, 43.4));
// put(9.0, SpeedAngleTriplet.of(3502.0, 3202.0, 40.2));
// put(10.0, SpeedAngleTriplet.of(3706.0, 3305.0, 37.8));
// put(11.0, SpeedAngleTriplet.of(3856.0, 3539.0, 34.5));
// put(12.0, SpeedAngleTriplet.of(3921.0, 3558.0, 33.3));
// put(13.0, SpeedAngleTriplet.of(4075.0, 3691.0, 30.9));
// put(14.0, SpeedAngleTriplet.of(4190.0, 3731.0, 29.4));
// put(15.0, SpeedAngleTriplet.of(4531.0, 4058.0, 28));
// put(16.0, SpeedAngleTriplet.of(4190.0, 3731.0, 26.7));

// BAYOU (FLIPPED)
// put(6.0, SpeedAngleTriplet.of(2850.0, 3007.0, 50.1));
// put(7.0, SpeedAngleTriplet.of(2865.0, 3160.0, 46.3));
// put(8.0, SpeedAngleTriplet.of(3017.0, 3310.0, 43.4));
// put(9.0, SpeedAngleTriplet.of(3202.0, 3502.0, 40.2));
// put(10.0, SpeedAngleTriplet.of(3305.0, 3706.0, 37.8));
// put(11.0, SpeedAngleTriplet.of(3539.0, 3856.0, 34.5));
// put(12.0, SpeedAngleTriplet.of(3558.0, 3921.0, 33.3));
// put(13.0, SpeedAngleTriplet.of(3691.0, 4075.0, 30.9));
// put(14.0, SpeedAngleTriplet.of(3731.0, 4190.0, 29.4));
// put(15.0, SpeedAngleTriplet.of(4058.0, 4531.0, 28));
// put(16.0, SpeedAngleTriplet.of(3731.0, 4190.0, 26.7));

}
};
Expand Down

0 comments on commit 4f16afc

Please sign in to comment.