Skip to content

Commit

Permalink
Merge branch 'main' into davis-CA-auto
Browse files Browse the repository at this point in the history
  • Loading branch information
@camearle20
camearle20 authored Apr 13, 2024
2 parents 05caade + a55356a commit 9cba813
Show file tree
Hide file tree
Showing 8 changed files with 186 additions and 29 deletions.
3 changes: 3 additions & 0 deletions src/main/java/org/littletonrobotics/frc2024/AutoSelector.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,6 +147,9 @@ public static enum AutoQuestionResponse {
SCORE_POOPED,
FOURTH_CENTER,
THINKING_ON_YOUR_FEET,
IMMEDIATELY,
SIX_SECONDS,
LAST_SECOND,
YES,
NO
}
Expand Down
26 changes: 22 additions & 4 deletions src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -381,6 +381,24 @@ private void configureAutos() {
"First center note?",
List.of(AutoQuestionResponse.SOURCE_WALL, AutoQuestionResponse.SOURCE_MIDDLE))),
autoBuilder.davisUnethicalAuto());
autoSelector.addRoutine(
"Davis Inspirational Auto",
List.of(
new AutoQuestion(
"Starting subwoofer location?",
List.of(
AutoQuestionResponse.SOURCE,
AutoQuestionResponse.CENTER,
AutoQuestionResponse.AMP)),
new AutoQuestion(
"Earn mobility bonus?", List.of(AutoQuestionResponse.YES, AutoQuestionResponse.NO)),
new AutoQuestion(
"Mobility delay time?",
List.of(
AutoQuestionResponse.IMMEDIATELY,
AutoQuestionResponse.SIX_SECONDS,
AutoQuestionResponse.LAST_SECOND))),
autoBuilder.davisInspirationalAuto());

// Set up feedforward characterization
autoSelector.addRoutine(
Expand Down Expand Up @@ -457,15 +475,15 @@ private void configureButtonBindings() {
Trigger nearSpeaker = new Trigger(robotState::inShootingZone);
driver
.a()
.and(nearSpeaker)
.and(nearSpeaker.or(shootPresets))
.whileTrue(
driveAimCommand
.get()
.alongWith(superstructureAimCommand.get(), flywheels.shootCommand())
.withName("Prepare Shot"));
driver
.a()
.and(nearSpeaker.negate())
.and(nearSpeaker.negate().and(shootPresets.negate()))
.whileTrue(
Commands.startEnd(
() ->
Expand All @@ -482,7 +500,7 @@ private void configureButtonBindings() {
driver
.rightTrigger()
.and(driver.a())
.and(nearSpeaker)
.and(nearSpeaker.or(shootPresets))
.and(readyToShoot.debounce(0.2, DebounceType.kRising))
.onTrue(
Commands.parallel(
Expand All @@ -494,7 +512,7 @@ private void configureButtonBindings() {
driver
.rightTrigger()
.and(driver.a())
.and(nearSpeaker.negate())
.and(nearSpeaker.negate().and(shootPresets.negate()))
.and(readyToShoot.debounce(0.3, DebounceType.kFalling))
.onTrue(
Commands.parallel(
Expand Down
41 changes: 29 additions & 12 deletions src/main/java/org/littletonrobotics/frc2024/RobotState.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ public record AimingParameters(
private static final LoggedTunableNumber lookahead =
new LoggedTunableNumber("RobotState/lookaheadS", 0.35);
private static final LoggedTunableNumber superPoopLookahead =
new LoggedTunableNumber("RobotState/SuperPoopLookahead", 1.0);
new LoggedTunableNumber("RobotState/SuperPoopLookahead", 0.1);
private static final LoggedTunableNumber closeShootingZoneFeet =
new LoggedTunableNumber("RobotState/CloseShootingZoneFeet", 10.0);
private static final double poseBufferSizeSeconds = 2.0;
Expand All @@ -73,18 +73,21 @@ public record AimingParameters(
new InterpolatingDoubleTreeMap();

static {
superPoopArmAngleMap.put(Units.feetToMeters(30.0), 35.0);
superPoopArmAngleMap.put(Units.feetToMeters(25.0), 37.0);
superPoopArmAngleMap.put(Units.feetToMeters(22.0), 45.0);
superPoopArmAngleMap.put(Units.feetToMeters(33.52713263758169), 35.0);
superPoopArmAngleMap.put(Units.feetToMeters(28.31299227120627), 39.0);
superPoopArmAngleMap.put(Units.feetToMeters(25.587026383435525), 48.0);
}

private static final InterpolatingTreeMap<Double, FlywheelSpeeds> superPoopFlywheelSpeedsMap =
new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), FlywheelSpeeds::interpolate);

static {
superPoopFlywheelSpeedsMap.put(Units.feetToMeters(30.0), new FlywheelSpeeds(3500, 4500));
superPoopFlywheelSpeedsMap.put(Units.feetToMeters(25.0), new FlywheelSpeeds(4100, 4100));
superPoopFlywheelSpeedsMap.put(Units.feetToMeters(22.0), new FlywheelSpeeds(2700, 3700));
superPoopFlywheelSpeedsMap.put(
Units.feetToMeters(33.52713263758169), new FlywheelSpeeds(3500, 4500));
superPoopFlywheelSpeedsMap.put(
Units.feetToMeters(28.31299227120627), new FlywheelSpeeds(2800, 3500));
superPoopFlywheelSpeedsMap.put(
Units.feetToMeters(25.587026383435525), new FlywheelSpeeds(2500, 3200));
}

private static final double autoFarShotCompensationDegrees = 0.0; // 0.6 at NECMP
Expand Down Expand Up @@ -282,7 +285,9 @@ public AimingParameters getAimingParameters() {
}

private static final Translation2d superPoopTarget =
FieldConstants.Stage.podiumLeg.getTranslation().interpolate(FieldConstants.ampCenter, 0.5);
FieldConstants.Subwoofer.centerFace
.getTranslation()
.interpolate(FieldConstants.ampCenter, 0.5);

public AimingParameters getSuperPoopAimingParameters() {
if (latestSuperPoopParameters != null) {
Expand All @@ -293,13 +298,25 @@ public AimingParameters getSuperPoopAimingParameters() {
Translation2d predictedRobotToTarget =
AllianceFlipUtil.apply(superPoopTarget).minus(predictedFieldToRobot.getTranslation());
double effectiveDistance = predictedRobotToTarget.getNorm();
var flywheelSpeeds = superPoopFlywheelSpeedsMap.get(effectiveDistance);
var armAngle = Rotation2d.fromDegrees(superPoopArmAngleMap.get(effectiveDistance));

Translation2d vehicleVelocity =
new Translation2d(robotVelocity.dx, robotVelocity.dy)
.rotateBy(predictedRobotToTarget.getAngle().unaryMinus());
Logger.recordOutput("RobotState/SuperPoopParameters/RadialVelocity", vehicleVelocity.getX());
double radialVelocity =
Units.radiansPerSecondToRotationsPerMinute(
vehicleVelocity.getX() / Units.inchesToMeters(1.5))
* armAngle.getCos();
flywheelSpeeds =
new FlywheelSpeeds(
flywheelSpeeds.leftSpeed() - radialVelocity,
flywheelSpeeds.rightSpeed() - radialVelocity);

latestSuperPoopParameters =
new AimingParameters(
predictedRobotToTarget.getAngle(),
Rotation2d.fromDegrees(superPoopArmAngleMap.get(effectiveDistance)),
effectiveDistance,
superPoopFlywheelSpeedsMap.get(effectiveDistance));
predictedRobotToTarget.getAngle(), armAngle, effectiveDistance, flywheelSpeeds);
return latestSuperPoopParameters;
}

Expand Down
71 changes: 71 additions & 0 deletions src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -756,4 +756,75 @@ private Command unethical_poopThenScoreCenterlines(
superstructure.aimWithCompensation(
secondShotCompensation)))))));
}

public Command davisInspirationalAuto() {
HolonomicTrajectory leaveFromSource = new HolonomicTrajectory("inspirational_leaveFromSource");
HolonomicTrajectory leaveFromCenter = new HolonomicTrajectory("inspirational_leaveFromCenter");
HolonomicTrajectory leaveFromAmp = new HolonomicTrajectory("inspirational_leaveFromAmp");
Timer autoTimer = new Timer();
return Commands.runOnce(autoTimer::restart)
.andThen(
Commands.parallel(
Commands.sequence(
waitUntilXCrossed(FieldConstants.startingLineX, true),
Commands.runOnce(
() -> System.out.println("Crossed starting line at " + autoTimer.get()))),
Commands.select(
Map.of(
AutoQuestionResponse.SOURCE,
resetPose(DriveTrajectories.startingSourceSubwoofer),
AutoQuestionResponse.CENTER,
resetPose(DriveTrajectories.startingCenter),
AutoQuestionResponse.AMP,
resetPose(DriveTrajectories.startingAmpSubwoofer)),
() -> responses.get().get(0) // Starting location
)
.andThen(
// Shoot preload in one second
Commands.waitSeconds(1.0 - shootTimeoutSecs.get())
.andThen(feed(rollers))
.deadlineWith(
flywheels.shootCommand(), superstructure.aimWithCompensation(0)),

// Wait time
Commands.select(
Map.of(
AutoQuestionResponse.IMMEDIATELY,
Commands.none(),
AutoQuestionResponse.SIX_SECONDS,
Commands.waitSeconds(5.0),
AutoQuestionResponse.LAST_SECOND,
Commands.select(
Map.of(
AutoQuestionResponse.SOURCE,
Commands.waitSeconds(
13.5 - leaveFromSource.getDuration()),
AutoQuestionResponse.CENTER,
Commands.waitSeconds(
13.5 - leaveFromCenter.getDuration()),
AutoQuestionResponse.AMP,
Commands.waitSeconds(
13.5 - leaveFromAmp.getDuration())),
() -> responses.get().get(0))),
() -> responses.get().get(2))
.andThen(
Commands.select(
Map.of(
AutoQuestionResponse.SOURCE,
followTrajectory(
drive,
new HolonomicTrajectory(
"inspirational_leaveFromSource")),
AutoQuestionResponse.CENTER,
followTrajectory(
drive,
new HolonomicTrajectory(
"inspirational_leaveFromCenter")),
AutoQuestionResponse.AMP,
followTrajectory(
drive,
new HolonomicTrajectory("inspirational_leaveFromAmp"))),
() -> responses.get().get(0)))
.onlyIf(() -> responses.get().get(1) == AutoQuestionResponse.YES))));
}
}
40 changes: 40 additions & 0 deletions ...ain/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
Expand Down Expand Up @@ -58,6 +59,13 @@ public class DriveTrajectories {
Rotation2d.fromDegrees(180.0));
public static final Pose2d startingFarSource =
new Pose2d(FieldConstants.startingLineX - 0.5, 1.57, Rotation2d.fromDegrees(180));
// Subwoofer starting locations
public static final Pose2d startingSourceSubwoofer =
FieldConstants.Subwoofer.sourceFaceCorner.transformBy(
GeomUtil.toTransform2d(-Units.inchesToMeters(17.0), Units.inchesToMeters(17.0)));
public static final Pose2d startingAmpSubwoofer =
FieldConstants.Subwoofer.ampFaceCorner.transformBy(
GeomUtil.toTransform2d(-Units.inchesToMeters(17.0), -Units.inchesToMeters(17.0)));

// Shooting poses
public static final Pose2d stageLeftShootingPose =
Expand Down Expand Up @@ -801,6 +809,38 @@ public class DriveTrajectories {
.build()));
}

// Davis Inspirational Auto (named "inspirational_XXX")
static {
paths.put(
"inspirational_leaveFromSource",
List.of(
PathSegment.newBuilder()
.addPoseWaypoint(startingSourceSubwoofer)
.addTranslationWaypoint(
FieldConstants.Stage.podiumLeg
.getTranslation()
.plus(new Translation2d(0.0, -2.0)))
.build()));
paths.put(
"inspirational_leaveFromCenter",
List.of(
PathSegment.newBuilder()
.addPoseWaypoint(startingCenter)
.addTranslationWaypoint(
FieldConstants.StagingLocations.spikeTranslations[1].plus(
new Translation2d(1.0, 0.0)))
.build()));
paths.put(
"inspirational_leaveFromAmp",
List.of(
PathSegment.newBuilder()
.addPoseWaypoint(startingAmpSubwoofer)
.addTranslationWaypoint(
FieldConstants.StagingLocations.spikeTranslations[2].plus(
new Translation2d(1.0, 0.0)))
.build()));
}

/** Calculates aimed pose from translation. */
private static Pose2d getShootingPose(Translation2d translation) {
return new Pose2d(
Expand Down
9 changes: 4 additions & 5 deletions src/main/java/org/littletonrobotics/frc2024/subsystems/leds/Leds.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,6 @@ public static Leds getInstance() {
private static final boolean prideLeds = false;
private static final int minLoopCycleCount = 10;
private static final int length = 12;
private static final int staticSectionLength = 3;
private static final double strobeDuration = 0.1;
private static final double breathDuration = 1.0;
private static final double rainbowCycleLength = 25.0;
Expand Down Expand Up @@ -159,7 +158,6 @@ public synchronized void periodic() {
new Color(0.15, 0.3, 1.0)),
3,
5.0);
buffer.setLED(staticSectionLength, allianceColor);
} else {
// Default pattern
wave(allianceColor, secondaryDisabledColor, waveAllianceCycleLength, waveAllianceDuration);
Expand Down Expand Up @@ -263,11 +261,12 @@ private void wave(Color c1, Color c2, double cycleLength, double duration) {
}
}

private void stripes(List<Color> colors, int length, double duration) {
int offset = (int) (Timer.getFPGATimestamp() % duration / duration * length * colors.size());
private void stripes(List<Color> colors, int stripeLength, double duration) {
int offset =
(int) (Timer.getFPGATimestamp() % duration / duration * stripeLength * colors.size());
for (int i = 0; i < length; i++) {
int colorIndex =
(int) (Math.floor((double) (i - offset) / length) + colors.size()) % colors.size();
(int) (Math.floor((double) (i - offset) / stripeLength) + colors.size()) % colors.size();
colorIndex = colors.size() - 1 - colorIndex;
buffer.setLED(i, colors.get(colorIndex));
}
Expand Down
21 changes: 15 additions & 6 deletions src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -108,6 +108,8 @@ private double getRads() {
private TrapezoidProfile.Constraints currentConstraints = maxProfileConstraints.get();
private TrapezoidProfile profile;
private TrapezoidProfile.State setpointState = new TrapezoidProfile.State();

private double goalAngle;
private ArmFeedforward ff;

private final ArmVisualizer measuredVisualizer;
Expand Down Expand Up @@ -202,7 +204,7 @@ public void periodic() {
// Don't run profile when characterizing, coast mode, or disabled
if (!characterizing && brakeModeEnabled && !disableSupplier.getAsBoolean()) {
// Run closed loop
double goalAngle =
goalAngle =
goal.getRads() + (goal == Goal.AIM ? Units.degreesToRadians(currentCompensation) : 0.0);
if (goal == Goal.STOW) {
goalAngle = getStowAngle();
Expand All @@ -217,15 +219,22 @@ public void periodic() {
Units.degreesToRadians(lowerLimitDegrees.get()),
Units.degreesToRadians(upperLimitDegrees.get())),
0.0));
io.runSetpoint(
setpointState.position, ff.calculate(setpointState.position, setpointState.velocity));
if (goal == Goal.STOW
&& EqualsUtil.epsilonEquals(goalAngle, minAngle.getRadians())
&& atGoal()) {
io.stop();
} else {
io.runSetpoint(
setpointState.position, ff.calculate(setpointState.position, setpointState.velocity));
}

goalVisualizer.update(goalAngle);
Logger.recordOutput("Arm/GoalAngle", goalAngle);
}

// Logs
measuredVisualizer.update(inputs.positionRads);
setpointVisualizer.update(setpointState.position);
goalVisualizer.update(goal.getRads());
Logger.recordOutput("Arm/GoalAngle", goal.getRads());
Logger.recordOutput("Arm/SetpointAngle", setpointState.position);
Logger.recordOutput("Arm/SetpointVelocity", setpointState.velocity);
Logger.recordOutput("Superstructure/Arm/Goal", goal);
Expand All @@ -237,7 +246,7 @@ public void stop() {

@AutoLogOutput(key = "Superstructure/Arm/AtGoal")
public boolean atGoal() {
return EqualsUtil.epsilonEquals(setpointState.position, goal.getRads(), 1e-3);
return EqualsUtil.epsilonEquals(setpointState.position, goalAngle, 1e-3);
}

public void setBrakeMode(boolean enabled) {
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,8 +46,8 @@ public class ArmConstants {
/** The offset of the arm encoder in radians. */
public static final double armEncoderOffsetRads =
switch (Constants.getRobot()) {
default -> 1.1886991875;
// corresponding to an arm position of 0.11965050145508001 rad
default -> 1.19023317;
// corresponding to an arm position of 0.1043106935762236 rad
case DEVBOT -> -1.233 - Math.PI / 2.0;
};

Expand Down

0 comments on commit 9cba813

Please sign in to comment.