Skip to content

Commit

Permalink
NYRO Friday
Browse files Browse the repository at this point in the history 
Co-Authored-By: Jack Jewell <[email protected]>
  • Loading branch information
@bryceroethel @JackJ09
bryceroethel and JackJ09 committed Mar 15, 2024
1 parent 3b7b9e8 commit 3134de9
Show file tree
Hide file tree
Showing 4 changed files with 40 additions and 43 deletions.
13 changes: 7 additions & 6 deletions src/main/java/org/team340/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ public final class Constants {
public static final class ControllerConstants {

public static final double DRIVE_EXP = 1.0;
public static final double DRIVE_MULTIPLIER = 0.9;
public static final double DRIVE_MULTIPLIER = 0.85;
public static final double DRIVE_MULTIPLIER_MODIFIED = 0.975;

public static final double DRIVE_ROT_EXP = 2.0;
Expand Down Expand Up @@ -364,7 +364,8 @@ public static final class Configs {
public static final class ShooterConstants {

// Speeds
public static final double RAMP_SPEED = 0.95;
public static final double RAMP_UP_SPEED = 0.95;
public static final double RAMP_DOWN_SPEED = 0.0;
public static final double INTAKE_HUMAN_SPEED = -0.175;
public static final double FIX_DEADZONE_SPEED = -0.5;
public static final double ROCK_SKIP_SPEED = 0.6;
Expand Down Expand Up @@ -514,10 +515,10 @@ public static final class SwerveConstants {
public static final double NORM_FUDGE = 0.49;
public static final double STRAFE_FUDGE = 0.85;
public static final double SPIN_COMPENSATION = Math.toRadians(-2.0);
public static final double DISTANCE_FUDGE_BLUE = 0.1;
public static final double DISTANCE_FUDGE_RED = 0.0;
public static final double DISTANCE_FUDGE_BLUE = 0.16;
public static final double DISTANCE_FUDGE_RED = 0.1;

public static final double NORM_FUDGE_MIN = 0.075; // .1
public static final double NORM_FUDGE_MIN = 0.075;
public static final double FACING_SPEAKER_EPSILON = Math.toRadians(5.0);

public static final double DRIVE_BASE_RADIUS = Math.hypot(FRONT_LEFT.getPosition().getX(), FRONT_LEFT.getPosition().getY());
Expand Down Expand Up @@ -545,7 +546,7 @@ public static final class FieldPositions {
Math2.ROTATION2D_NEG_HALF_PI
);
public static final Pose2d AMP_SCORE_BLUE = new Pose2d(AMP_X, 7.7, Math2.ROTATION2D_HALF_PI);
public static final Pose2d AMP_SCORE_RED = new Pose2d(AMP_X, FIELD_WIDTH - AMP_SCORE_BLUE.getY(), Math2.ROTATION2D_NEG_HALF_PI);
public static final Pose2d AMP_SCORE_RED = new Pose2d(AMP_X, 0.48, Math2.ROTATION2D_NEG_HALF_PI);

public static final Translation2d STAGE = new Translation2d(4.981067, 4.105783);

Expand Down
17 changes: 10 additions & 7 deletions src/main/java/org/team340/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ private static void configBindings() {
intake.setDefaultCommand(intake.maintainPosition());
lights.setDefaultCommand(lights.defaultCommand(intake::hasNote, feeder::hasNote));
pivot.setDefaultCommand(pivot.maintainPosition());
shooter.setDefaultCommand(shooter.targetDistance(swerve::getSpeakerDistance, 3000.0, swerve::inOpponentWing));
shooter.setDefaultCommand(shooter.targetDistance(swerve::getSpeakerDistance, 2750.0, swerve::inOpponentWing));
swerve.setDefaultCommand(swerve.drive(RobotContainer::getDriveX, RobotContainer::getDriveY, RobotContainer::getDriveRotate, true));

Routines.onDisable().schedule();
Expand All @@ -111,7 +111,7 @@ private static void configBindings() {
driver.b().onTrue(Routines.intakeHuman(RobotContainer::getDriveX, RobotContainer::getDriveY)).onFalse(Routines.finishIntakeHuman());

// X => Prep Amp (Hold)
driver.x().whileTrue(Routines.scoreAmp(RobotContainer::getDriveX, RobotContainer::getDriveY));
driver.x().whileTrue(Routines.prepAmp(RobotContainer::getDriveX, RobotContainer::getDriveY));

// Y => Shoot (Tap)
driver.y().whileTrue(feeder.shoot());
Expand Down Expand Up @@ -171,21 +171,24 @@ private static void configBindings() {
// X => Fender Shot (Hold)
coDriver.x().whileTrue(pivot.targetDistance(() -> FieldPositions.FENDER_SHOT_DISTANCE));

// Y => Fix deadzone (Hold)
coDriver.y().onTrue(Routines.fixDeadzone());
// Y => Score Amp (Hold)
coDriver.y().whileTrue(intake.scoreAmp());

// Both Triggers => Drives climber manually
coDriver
.leftTrigger()
.and(coDriver.rightTrigger())
.whileTrue(climber.driveManual(() -> -coDriver.getLeftY() * 0.3, () -> -coDriver.getRightY() * 0.3));

// POV Up => Prep Speaker (Hold)
coDriver.povUp().whileTrue(Routines.prepSpeaker(RobotContainer::getDriveX, RobotContainer::getDriveY));
// POV Up => Fix Deadzone (Hold)
coDriver.povUp().whileTrue(Routines.fixDeadzone());

// POV Down => Pivot home (Hold)
// POV Down => Pivot Home (Hold)
coDriver.povDown().whileTrue(pivot.home(true));

// POV Left => Prep Speaker (Hold)
coDriver.povLeft().whileTrue(Routines.prepSpeaker(RobotContainer::getDriveX, RobotContainer::getDriveY));

// Back / Start => he he rumble rumble
coDriver.back().toggleOnTrue(setCoDriverRumble(RumbleType.kLeftRumble, 0.5).ignoringDisable(true));
coDriver.start().toggleOnTrue(setCoDriverRumble(RumbleType.kRightRumble, 0.5).ignoringDisable(true));
Expand Down
43 changes: 15 additions & 28 deletions src/main/java/org/team340/robot/commands/Routines.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,6 @@

import edu.wpi.first.wpilibj2.command.Command;
import java.util.function.Supplier;
import org.team340.lib.util.Mutable;
import org.team340.robot.Constants;
import org.team340.robot.Constants.PivotConstants;

Expand Down Expand Up @@ -80,34 +79,26 @@ public static Command prepSpeaker(Supplier<Double> x, Supplier<Double> y) {
}

/**
* Automatically drives to the amp and runs the intake rollers to score,
* after the robot is in position the drive can manually drive the robot to adjust.
* Prepares to score in the amp.
* @param x The desired {@code x} driving speed from {@code -1.0} to {@code 1.0}.
* @param y The desired {@code y} driving speed from {@code -1.0} to {@code 1.0}.
*/
public static Command scoreAmp(Supplier<Double> x, Supplier<Double> y) {
Mutable<Boolean> approached = new Mutable<>(false);
return sequence(
runOnce(() -> approached.set(false)),
parallel(
swerve.driveAmp(true).until(() -> swerve.getAmpDistance() < 0.5).finallyDo(() -> approached.set(true)),
public static Command prepAmp(Supplier<Double> x, Supplier<Double> y) {
return parallel(
swerve.driveAmpManual(x, y),
sequence(
sequence(
sequence(
parallel(
pivot.goTo(Constants.PivotConstants.AMP_HANDOFF_POSITION),
sequence(waitUntil(pivot::isSafeForIntake), intake.handoffPosition())
),
handoff()
)
.unless(intake::hasNote),
intake.ampPosition(),
intake.maintainPosition().until(approached::get)
parallel(
pivot.goTo(Constants.PivotConstants.AMP_HANDOFF_POSITION),
sequence(waitUntil(pivot::isSafeForIntake), intake.handoffPosition())
),
handoff()
)
),
swerve.driveAmp(false).until(() -> swerve.getAmpDistance() < 0.02).withTimeout(0.8),
parallel(intake.scoreAmp(), swerve.driveAmpManual(x, y))
.unless(intake::hasNote),
intake.ampPosition()
)
)
.withName("Routines.scoreAmp()");
.withName("Routines.prepAmp()");
}

/**
Expand All @@ -116,11 +107,7 @@ public static Command scoreAmp(Supplier<Double> x, Supplier<Double> y) {
* @param y The desired {@code y} driving speed from {@code -1.0} to {@code 1.0}.
*/
public static Command prepClimb(Supplier<Double> x, Supplier<Double> y) {
return parallel(
swerve.driveClimb(x, y),
intake.uprightPosition().onlyIf(() -> swerve.getStageDistance() >= 1.9),
pivot.goTo(PivotConstants.DOWN_POSITION)
)
return parallel(swerve.driveClimb(x, y), intake.uprightPosition(), pivot.goTo(PivotConstants.DOWN_POSITION))
.withName("Routines.prepClimb()");
}

Expand Down
10 changes: 8 additions & 2 deletions src/main/java/org/team340/robot/subsystems/Shooter.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -144,7 +144,10 @@ private void applySpeed(double speed) {
leftShootPID.setReference(leftSpeed, ControlType.kVelocity, 0, leftFeedForward.calculate(leftSpeed));
leftPIDActive = true;
} else {
leftShootMotor.set(ShooterConstants.RAMP_SPEED * Math.signum(leftDelta));
leftShootMotor.set(
(leftDelta * Math.signum(leftSpeed) > 0.0 ? ShooterConstants.RAMP_UP_SPEED : ShooterConstants.RAMP_DOWN_SPEED) *
Math.signum(leftSpeed)
);
leftPIDActive = false;
}

Expand All @@ -153,7 +156,10 @@ private void applySpeed(double speed) {
rightShootPID.setReference(rightSpeed, ControlType.kVelocity, 0, rightFeedForward.calculate(rightSpeed));
rightPIDActive = true;
} else {
rightShootMotor.set(ShooterConstants.RAMP_SPEED * Math.signum(rightDelta));
rightShootMotor.set(
(rightDelta * Math.signum(rightSpeed) > 0.0 ? ShooterConstants.RAMP_UP_SPEED : ShooterConstants.RAMP_DOWN_SPEED) *
Math.signum(rightSpeed)
);
rightPIDActive = false;
}
}
Expand Down

0 comments on commit 3134de9

Please sign in to comment.