From 42758bcf758a2ab4e2af12ced48f7ce00cffc915 Mon Sep 17 00:00:00 2001 From: Cameron Earle Date: Sat, 13 Apr 2024 16:20:01 -0400 Subject: [PATCH 1/4] Recalibrate arm encoder --- .../frc2024/subsystems/superstructure/arm/ArmConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java index f2bd9f02..0de2fa1e 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java @@ -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; }; From e129597fa3e40bbe830515b18c6504b0ffd85715 Mon Sep 17 00:00:00 2001 From: Neel <87990699+harnwalN@users.noreply.github.com> Date: Sat, 13 Apr 2024 16:23:54 -0400 Subject: [PATCH 2/4] =?UTF-8?q?Add=20Texas=20mode=20=F0=9F=A4=A0=20(#260)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fix stripe loop logic * Formatting * Disable pride by default --------- Co-authored-by: Jonah <47046556+jwbonner@users.noreply.github.com> --- .../littletonrobotics/frc2024/subsystems/leds/Leds.java | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/leds/Leds.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/leds/Leds.java index ed6f36d1..452a9238 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/leds/Leds.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/leds/Leds.java @@ -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; @@ -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); @@ -263,11 +261,12 @@ private void wave(Color c1, Color c2, double cycleLength, double duration) { } } - private void stripes(List colors, int length, double duration) { - int offset = (int) (Timer.getFPGATimestamp() % duration / duration * length * colors.size()); + private void stripes(List 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)); } From b510c630a554da260996c48cd6a790be6ab63975 Mon Sep 17 00:00:00 2001 From: Surya Thoppae <63197592+suryatho@users.noreply.github.com> Date: Sat, 13 Apr 2024 16:40:07 -0400 Subject: [PATCH 3/4] Tune super poop (#259) * Add inspirational auto * check style * Always un power arm at min angle and stow * Use epsilonEquals for goal check --------- Co-authored-by: Cameron Earle --- .../frc2024/RobotContainer.java | 8 ++-- .../littletonrobotics/frc2024/RobotState.java | 41 +++++++++++++------ .../subsystems/superstructure/arm/Arm.java | 21 +++++++--- 3 files changed, 48 insertions(+), 22 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 3c76dc5b..fb6f84e0 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -451,7 +451,7 @@ private void configureButtonBindings() { Trigger nearSpeaker = new Trigger(robotState::inShootingZone); driver .a() - .and(nearSpeaker) + .and(nearSpeaker.or(shootPresets)) .whileTrue( driveAimCommand .get() @@ -459,7 +459,7 @@ private void configureButtonBindings() { .withName("Prepare Shot")); driver .a() - .and(nearSpeaker.negate()) + .and(nearSpeaker.negate().and(shootPresets.negate())) .whileTrue( Commands.startEnd( () -> @@ -476,7 +476,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( @@ -488,7 +488,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( diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java index d30f4551..2c28467f 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotState.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotState.java @@ -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; @@ -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 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 @@ -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) { @@ -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; } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java index 5446fa70..fd0be936 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java @@ -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; @@ -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(); @@ -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); @@ -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) { From a55356ac5fbb9d6c2ad703432aed9bc110f70658 Mon Sep 17 00:00:00 2001 From: Surya Thoppae <63197592+suryatho@users.noreply.github.com> Date: Sat, 13 Apr 2024 17:13:22 -0400 Subject: [PATCH 4/4] Davis inspirational auto (#258) * Add inspirational auto * Critical no comma error * Better source trajectory * Replace 14 second option with last second initiation * Remove debug log * Adjustments --------- Co-authored-by: nharnwal Co-authored-by: Jonah <47046556+jwbonner@users.noreply.github.com> --- .../frc2024/AutoSelector.java | 7 +- .../frc2024/RobotContainer.java | 18 +++++ .../frc2024/commands/auto/AutoBuilder.java | 71 +++++++++++++++++++ .../drive/trajectory/DriveTrajectories.java | 40 +++++++++++ 4 files changed, 135 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/AutoSelector.java b/src/main/java/org/littletonrobotics/frc2024/AutoSelector.java index 0fc9e084..23adc493 100644 --- a/src/main/java/org/littletonrobotics/frc2024/AutoSelector.java +++ b/src/main/java/org/littletonrobotics/frc2024/AutoSelector.java @@ -146,6 +146,11 @@ public static enum AutoQuestionResponse { AMP_WALL, SCORE_POOPED, FOURTH_CENTER, - THINKING_ON_YOUR_FEET + THINKING_ON_YOUR_FEET, + IMMEDIATELY, + SIX_SECONDS, + LAST_SECOND, + YES, + NO } } diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index fb6f84e0..bc307928 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -375,6 +375,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( diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java b/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java index a36182e5..2aa80592 100644 --- a/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java +++ b/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java @@ -695,4 +695,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)))); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java index 62dc68b3..ffd8235b 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java @@ -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; @@ -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 = @@ -718,6 +726,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(