diff --git a/.github/workflows/vts.yml b/.github/workflows/vts.yml index 55be9147..ccb3ecf2 100644 --- a/.github/workflows/vts.yml +++ b/.github/workflows/vts.yml @@ -2,10 +2,14 @@ name: VTS Build on: push: - pull_request: + paths: + - 'trajectory_native/**' concurrency: - group: vts + group: ${{ github.workflow }}-${{ github.ref }} + +permissions: + packages: write jobs: build: diff --git a/robot-image.png b/robot-image.png index bbfc9d24..763b30e5 100644 Binary files a/robot-image.png and b/robot-image.png differ diff --git a/src/main/deploy/apriltags/2024-amps.json b/src/main/deploy/apriltags/2024-amps.json new file mode 100644 index 00000000..ddaf4be3 --- /dev/null +++ b/src/main/deploy/apriltags/2024-amps.json @@ -0,0 +1,44 @@ +{ + "tags": [ + { + "ID": 5, + "pose": { + "translation": { + "x": 14.700757999999999, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 1.8415, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + } + ], + "field": { + "length": 16.541, + "width": 8.211 + } +} diff --git a/src/main/deploy/apriltags/2024-official.json b/src/main/deploy/apriltags/2024-official.json new file mode 100644 index 00000000..8cb837a5 --- /dev/null +++ b/src/main/deploy/apriltags/2024-official.json @@ -0,0 +1,296 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 15.079471999999997, + "y": 0.24587199999999998, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 16.185134, + "y": 0.883666, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 16.579342, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 16.579342, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 14.700757999999999, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 1.8415, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 0.356108, + "y": 0.883666, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 1.4615159999999998, + "y": 0.24587199999999998, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 11.904726, + "y": 3.7132259999999997, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": -0.8660254037844387, + "X": -0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 11.904726, + "y": 4.49834, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 11.220196, + "y": 4.105148, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 5.320792, + "y": 4.105148, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 4.641342, + "y": 4.49834, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 4.641342, + "y": 3.7132259999999997, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": -0.4999999999999998, + "X": -0.0, + "Y": 0.0, + "Z": 0.8660254037844387 + } + } + } + } + ], + "field": { + "length": 16.541, + "width": 8.211 + } +} diff --git a/src/main/deploy/apriltags/2024-speakers.json b/src/main/deploy/apriltags/2024-speakers.json new file mode 100644 index 00000000..36ecc9df --- /dev/null +++ b/src/main/deploy/apriltags/2024-speakers.json @@ -0,0 +1,80 @@ +{ + "tags": [ + { + "ID": 3, + "pose": { + "translation": { + "x": 16.579342, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 16.579342, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + } + ], + "field": { + "length": 16.541, + "width": 8.211 + } +} diff --git a/src/main/deploy/apriltags/2024-wpi-custom.json b/src/main/deploy/apriltags/2024-wpi.json similarity index 100% rename from src/main/deploy/apriltags/2024-wpi-custom.json rename to src/main/deploy/apriltags/2024-wpi.json diff --git a/src/main/java/org/littletonrobotics/frc2024/Constants.java b/src/main/java/org/littletonrobotics/frc2024/Constants.java index 7ab9c60c..8ddc6324 100644 --- a/src/main/java/org/littletonrobotics/frc2024/Constants.java +++ b/src/main/java/org/littletonrobotics/frc2024/Constants.java @@ -7,15 +7,7 @@ package org.littletonrobotics.frc2024; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; -import java.io.IOException; -import java.nio.file.Path; -import java.util.function.Supplier; -import lombok.Getter; -import lombok.RequiredArgsConstructor; import org.littletonrobotics.frc2024.util.Alert; import org.littletonrobotics.frc2024.util.Alert.AlertType; @@ -32,8 +24,6 @@ public final class Constants { private static RobotType robotType = RobotType.COMPBOT; public static final boolean tuningMode = false; - public static final AprilTagType aprilTagType = AprilTagType.OFFICIAL; - public static RobotType getRobot() { if (!disableHAL && RobotBase.isReal() && robotType == RobotType.SIMBOT) { new Alert("Invalid robot selected, using competition robot as default.", AlertType.ERROR) @@ -50,27 +40,6 @@ public static Mode getMode() { }; } - @Getter - @RequiredArgsConstructor - public enum AprilTagType { - OFFICIAL(() -> AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo)), - WPI( - () -> { - if (disableHAL) return null; - try { - return new AprilTagFieldLayout( - Path.of( - Filesystem.getDeployDirectory().getPath(), - "apriltags", - "2024-wpi-custom.json")); - } catch (IOException e) { - throw new RuntimeException(e); - } - }); - - private final Supplier layoutSupplier; - } - public enum Mode { /** Running on a real robot. */ REAL, diff --git a/src/main/java/org/littletonrobotics/frc2024/FieldConstants.java b/src/main/java/org/littletonrobotics/frc2024/FieldConstants.java index 5d34598f..eca2d178 100644 --- a/src/main/java/org/littletonrobotics/frc2024/FieldConstants.java +++ b/src/main/java/org/littletonrobotics/frc2024/FieldConstants.java @@ -7,9 +7,15 @@ package org.littletonrobotics.frc2024; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.ObjectMapper; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Filesystem; +import java.io.IOException; +import java.nio.file.Path; +import lombok.Getter; /** * Contains various field dimensions and useful reference points. Dimensions are in meters, and sets @@ -139,9 +145,40 @@ public static final class Stage { } public static final double aprilTagWidth = Units.inchesToMeters(6.50); - public static final AprilTagFieldLayout aprilTags; + public static final AprilTagLayoutType defaultAprilTagType = AprilTagLayoutType.OFFICIAL; + + @Getter + public enum AprilTagLayoutType { + OFFICIAL("2024-official"), + SPEAKERS_ONLY("2024-speakers"), + AMPS_ONLY("2024-amps"), + WPI("2024-wpi"); + + private AprilTagLayoutType(String name) { + if (Constants.disableHAL) { + layout = null; + } else { + try { + layout = + new AprilTagFieldLayout( + Path.of(Filesystem.getDeployDirectory().getPath(), "apriltags", name + ".json")); + } catch (IOException e) { + throw new RuntimeException(e); + } + } + if (layout == null) { + layoutString = ""; + } else { + try { + layoutString = new ObjectMapper().writeValueAsString(layout); + } catch (JsonProcessingException e) { + throw new RuntimeException( + "Failed to serialize AprilTag layout JSON " + toString() + "for Northstar"); + } + } + } - static { - aprilTags = Constants.aprilTagType.getLayoutSupplier().get(); + private final AprilTagFieldLayout layout; + private final String layoutString; } } diff --git a/src/main/java/org/littletonrobotics/frc2024/Robot.java b/src/main/java/org/littletonrobotics/frc2024/Robot.java index 08f1f1e6..ba09d3e6 100644 --- a/src/main/java/org/littletonrobotics/frc2024/Robot.java +++ b/src/main/java/org/littletonrobotics/frc2024/Robot.java @@ -82,9 +82,7 @@ public class Robot extends LoggedRobot { public void robotInit() { // Record metadata Logger.recordMetadata("Robot", Constants.getRobot().toString()); - System.out.println("[Init] Scanning battery"); Logger.recordMetadata("BatteryName", "BAT-" + BatteryTracker.scanBattery(1.5)); - System.out.println("[Init] Starting AdvantageKit"); Logger.recordMetadata("TuningMode", Boolean.toString(Constants.tuningMode)); Logger.recordMetadata("RuntimeType", getRuntimeType().toString()); Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); @@ -234,6 +232,7 @@ public void robotPeriodic() { // Robot container periodic methods robotContainer.checkControllers(); robotContainer.updateDashboardOutputs(); + robotContainer.updateAprilTagAlert(); // Update NoteVisualizer NoteVisualizer.showHeldNotes(); diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index dfa8e5f8..d461dc54 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -7,6 +7,7 @@ package org.littletonrobotics.frc2024; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -22,10 +23,12 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; import java.util.function.Function; import java.util.function.Supplier; import lombok.experimental.ExtensionMethod; +import org.littletonrobotics.frc2024.FieldConstants.AprilTagLayoutType; import org.littletonrobotics.frc2024.commands.ClimbingCommands; import org.littletonrobotics.frc2024.commands.FeedForwardCharacterization; import org.littletonrobotics.frc2024.commands.StaticCharacterization; @@ -100,10 +103,13 @@ public class RobotContainer { private final Trigger robotRelative = overrides.driverSwitch(0); private final Trigger armDisable = overrides.driverSwitch(1); private final Trigger armCoast = overrides.driverSwitch(2); + private final Trigger aprilTagsSpeakerOnly = overrides.multiDirectionSwitchLeft(); + private final Trigger aprilTagsAmpOnly = overrides.multiDirectionSwitchRight(); private final Trigger shootPresets = overrides.operatorSwitch(0); private final Trigger shootAlignDisable = overrides.operatorSwitch(1); private final Trigger lookaheadDisable = overrides.operatorSwitch(2); private final Trigger autoDriveDisable = overrides.operatorSwitch(3); + private final Alert aprilTagLayoutAlert = new Alert("", AlertType.INFO); private final Alert driverDisconnected = new Alert("Driver controller disconnected (port 0).", AlertType.WARNING); private final Alert operatorDisconnected = @@ -117,11 +123,23 @@ public class RobotContainer { private boolean podiumShotMode = false; private boolean trapScoreMode = true; + private boolean armCoastOverride = false; // Dashboard inputs private final LoggedDashboardChooser autoChooser = new LoggedDashboardChooser<>("Auto Choices"); + /** Returns the current AprilTag layout type. */ + public AprilTagLayoutType getAprilTagLayoutType() { + if (aprilTagsSpeakerOnly.getAsBoolean()) { + return FieldConstants.AprilTagLayoutType.SPEAKERS_ONLY; + } else if (aprilTagsAmpOnly.getAsBoolean()) { + return FieldConstants.AprilTagLayoutType.AMPS_ONLY; + } else { + return FieldConstants.defaultAprilTagType; + } + } + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Declare component subsystems (not visible outside constructor) @@ -147,10 +165,11 @@ public RobotContainer() { new ModuleIOKrakenFOC(DriveConstants.moduleConfigs[3])); aprilTagVision = new AprilTagVision( - new AprilTagVisionIONorthstar(0), - new AprilTagVisionIONorthstar(1), - new AprilTagVisionIONorthstar(2), - new AprilTagVisionIONorthstar(3)); + this::getAprilTagLayoutType, + new AprilTagVisionIONorthstar(this::getAprilTagLayoutType, 0), + new AprilTagVisionIONorthstar(this::getAprilTagLayoutType, 1), + new AprilTagVisionIONorthstar(this::getAprilTagLayoutType, 2), + new AprilTagVisionIONorthstar(this::getAprilTagLayoutType, 3)); flywheels = new Flywheels(new FlywheelsIOKrakenFOC()); feeder = new Feeder(new FeederIOKrakenFOC()); indexer = new Indexer(new IndexerIOCompbot()); @@ -171,7 +190,9 @@ public RobotContainer() { new ModuleIOSparkMax(DriveConstants.moduleConfigs[3])); aprilTagVision = new AprilTagVision( - new AprilTagVisionIONorthstar(0), new AprilTagVisionIONorthstar(1)); + this::getAprilTagLayoutType, + new AprilTagVisionIONorthstar(this::getAprilTagLayoutType, 0), + new AprilTagVisionIONorthstar(this::getAprilTagLayoutType, 1)); flywheels = new Flywheels(new FlywheelsIOSparkFlex()); feeder = new Feeder(new FeederIOKrakenFOC()); indexer = new Indexer(new IndexerIODevbot()); @@ -216,14 +237,18 @@ public RobotContainer() { case COMPBOT -> aprilTagVision = new AprilTagVision( + this::getAprilTagLayoutType, new AprilTagVisionIO() {}, new AprilTagVisionIO() {}, new AprilTagVisionIO() {}, new AprilTagVisionIO() {}); case DEVBOT -> aprilTagVision = - new AprilTagVision(new AprilTagVisionIO() {}, new AprilTagVisionIO() {}); - default -> aprilTagVision = new AprilTagVision(); + new AprilTagVision( + this::getAprilTagLayoutType, + new AprilTagVisionIO() {}, + new AprilTagVisionIO() {}); + default -> aprilTagVision = new AprilTagVision(this::getAprilTagLayoutType); } } if (flywheels == null) { @@ -257,9 +282,22 @@ public RobotContainer() { superstructure = new Superstructure(arm, climber, backpackActuator); // Set up subsystems - arm.setOverrides(armDisable, armCoast); + armCoast + .onTrue( + Commands.runOnce( + () -> { + if (DriverStation.isDisabled()) { + armCoastOverride = true; + } + }) + .ignoringDisable(true)) + .onFalse(Commands.runOnce(() -> armCoastOverride = false).ignoringDisable(true)); + RobotModeTriggers.disabled() + .onFalse(Commands.runOnce(() -> armCoastOverride = false).ignoringDisable(true)); + arm.setOverrides(armDisable, () -> armCoastOverride); + climber.setCoastOverride(() -> armCoastOverride); + backpackActuator.setCoastOverride(() -> armCoastOverride); RobotState.getInstance().setLookaheadDisable(lookaheadDisable); - climber.setCoastOverride(armCoast); flywheels.setPrepareShootSupplier( () -> { return DriverStation.isTeleopEnabled() @@ -283,9 +321,6 @@ public RobotContainer() { configureButtonBindings(); // Alerts for constants - if (Constants.aprilTagType != Constants.AprilTagType.OFFICIAL) { - new Alert("Non-official AprilTag layout selected", AlertType.INFO).set(true); - } if (Constants.tuningMode) { new Alert("Tuning mode enabled", AlertType.INFO).set(true); } @@ -411,13 +446,29 @@ private void configureButtonBindings() { () -> RobotState.getInstance().getAimingParameters().driveHeading()), drive::clearHeadingGoal), shootAlignDisable); + Trigger inWing = + new Trigger( + () -> + AllianceFlipUtil.apply(RobotState.getInstance().getEstimatedPose().getX()) + < FieldConstants.wingX); driver .a() + .and(inWing) .whileTrue( driveAimCommand .get() .alongWith(superstructureAimCommand.get(), flywheels.shootCommand()) .withName("Prepare Shot")); + driver + .a() + .and(inWing.negate()) + .whileTrue( + driveAimCommand + .get() + .alongWith( + superstructure.setGoalCommand(Superstructure.Goal.SUPER_POOP), + flywheels.superPoopCommand()) + .withName("Super Poop")); Trigger readyToShoot = new Trigger( () -> drive.atHeadingGoal() && superstructure.atArmGoal() && flywheels.atGoal()); @@ -444,6 +495,16 @@ private void configureButtonBindings() { driver.getHID().setRumble(RumbleType.kBothRumble, 0.0); })); + // Poop. + driver + .y() + .whileTrue( + flywheels + .poopCommand() + .alongWith( + Commands.waitUntil(flywheels::atGoal) + .andThen(rollers.setGoalCommand(Rollers.Goal.FEED_TO_SHOOTER)))); + // ------------- Intake Controls ------------- // Intake Floor driver @@ -483,11 +544,15 @@ private void configureButtonBindings() { Pose2d ampCenterRotated = AllianceFlipUtil.apply( new Pose2d(FieldConstants.ampCenter, new Rotation2d(-Math.PI / 2.0))); - return ampCenterRotated.transformBy( - GeomUtil.toTransform2d( - Units.inchesToMeters(20.0) // End of intake bumper to center robot - + Units.inchesToMeters(9.0), - 0)); + var finalPose = + ampCenterRotated.transformBy(GeomUtil.toTransform2d(Units.inchesToMeters(20.0), 0)); + double distance = + RobotState.getInstance() + .getEstimatedPose() + .getTranslation() + .getDistance(finalPose.getTranslation()); + double offsetT = MathUtil.clamp((distance - 0.5) / 2.5, 0.0, 1.0); + return finalPose.transformBy(GeomUtil.toTransform2d(offsetT * 1.5, 0.0)); }; driver .b() @@ -574,6 +639,7 @@ private void configureButtonBindings() { operator.rightStick().onTrue(Commands.runOnce(() -> trapScoreMode = !trapScoreMode)); operator .leftBumper() + .doublePress() .and(() -> trapScoreMode) .toggleOnTrue( ClimbingCommands.climbNTrapSequence( @@ -589,6 +655,7 @@ private void configureButtonBindings() { .withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); operator .leftBumper() + .doublePress() .and(() -> !trapScoreMode) .toggleOnTrue( ClimbingCommands.simpleClimbSequence( @@ -672,6 +739,16 @@ public void updateDashboardOutputs() { SmartDashboard.putBoolean("Trap Score Mode", trapScoreMode); } + /** Updates the AprilTag alert. */ + public void updateAprilTagAlert() { + boolean active = getAprilTagLayoutType() != AprilTagLayoutType.OFFICIAL; + aprilTagLayoutAlert.set(active); + if (active) { + aprilTagLayoutAlert.setText( + "Non-official AprilTag layout in use (" + getAprilTagLayoutType().toString() + ")."); + } + } + /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java index 8cd8ee62..4d32e716 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotState.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotState.java @@ -54,27 +54,20 @@ public record AimingParameters( @AutoLogOutput @Getter @Setter private boolean flywheelAccelerating = false; static { - armAngleMap.put(1.026639, 52.0); - armAngleMap.put(1.156125, 50.0); - armAngleMap.put(1.174623, 50.0); - armAngleMap.put(1.38735, 49.0); - armAngleMap.put(1.618575, 44.0); - armAngleMap.put(1.8498, 40.5); - armAngleMap.put(2.081025, 37.5); - armAngleMap.put(2.31225, 36.0); - armAngleMap.put(2.543475, 34.0); - armAngleMap.put(2.7747, 33.0); - armAngleMap.put(3.005925, 31.0); - armAngleMap.put(3.23715, 30.0); - armAngleMap.put(3.468375, 28.0); - armAngleMap.put(3.6996, 27.5); - armAngleMap.put(3.912327, 26.5); - armAngleMap.put(4.16205, 26.25); - armAngleMap.put(4.393275, 25.25); - armAngleMap.put(4.6245, 25.0); - armAngleMap.put(4.855725, 24.75); - armAngleMap.put(5.170191, 24.6); - armAngleMap.put(5.373669, 24.25); + armAngleMap.put(1.22, 52.5); + armAngleMap.put(1.39, 47.5); + armAngleMap.put(1.66, 43.5); + armAngleMap.put(2.0, 40.5); + armAngleMap.put(2.13, 38.5); + armAngleMap.put(2.51, 33.5); + armAngleMap.put(2.79, 31.5); + armAngleMap.put(2.94, 29.5); + armAngleMap.put(3.38, 26.5); + armAngleMap.put(3.75, 24.5); + armAngleMap.put(4.06, 23.7); + armAngleMap.put(4.42, 22.8); + armAngleMap.put(4.89, 22.0); + armAngleMap.put(5.2, 21.5); } @AutoLogOutput @Getter @Setter private double shotCompensationDegrees = 0.0; diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVision.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVision.java index a5fcf4ed..38908211 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVision.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVision.java @@ -11,7 +11,6 @@ import static org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionConstants.*; import static org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionIO.AprilTagVisionIOInputs; -import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -20,8 +19,10 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.wpilibj.Timer; import java.util.*; +import java.util.function.Supplier; import lombok.experimental.ExtensionMethod; import org.littletonrobotics.frc2024.FieldConstants; +import org.littletonrobotics.frc2024.FieldConstants.AprilTagLayoutType; import org.littletonrobotics.frc2024.RobotState; import org.littletonrobotics.frc2024.util.GeomUtil; import org.littletonrobotics.frc2024.util.VirtualSubsystem; @@ -31,13 +32,15 @@ @ExtensionMethod({GeomUtil.class}) public class AprilTagVision extends VirtualSubsystem { + private final Supplier aprilTagTypeSupplier; private final AprilTagVisionIO[] io; private final AprilTagVisionIOInputs[] inputs; private final Map lastFrameTimes = new HashMap<>(); private final Map lastTagDetectionTimes = new HashMap<>(); - public AprilTagVision(AprilTagVisionIO... io) { + public AprilTagVision(Supplier aprilTagTypeSupplier, AprilTagVisionIO... io) { + this.aprilTagTypeSupplier = aprilTagTypeSupplier; this.io = io; inputs = new AprilTagVisionIOInputs[io.length]; for (int i = 0; i < io.length; i++) { @@ -48,14 +51,6 @@ public AprilTagVision(AprilTagVisionIO... io) { for (int i = 0; i < io.length; i++) { lastFrameTimes.put(i, 0.0); } - - // Create map of last detection times for tags - FieldConstants.aprilTags - .getTags() - .forEach( - (AprilTag tag) -> { - lastTagDetectionTimes.put(tag.ID, 0.0); - }); } public void periodic() { @@ -159,9 +154,11 @@ public void periodic() { for (int i = (values[0] == 1 ? 9 : 17); i < values.length; i++) { int tagId = (int) values[i]; lastTagDetectionTimes.put(tagId, Timer.getFPGATimestamp()); - Optional tagPose = FieldConstants.aprilTags.getTagPose((int) values[i]); + Optional tagPose = + aprilTagTypeSupplier.get().getLayout().getTagPose((int) values[i]); tagPose.ifPresent(tagPoses::add); } + if (tagPoses.size() == 0) continue; // Calculate average distance to tag double totalDistance = 0.0; @@ -220,7 +217,11 @@ public void periodic() { List allTagPoses = new ArrayList<>(); for (Map.Entry detectionEntry : lastTagDetectionTimes.entrySet()) { if (Timer.getFPGATimestamp() - detectionEntry.getValue() < targetLogTimeSecs) { - allTagPoses.add(FieldConstants.aprilTags.getTagPose(detectionEntry.getKey()).get()); + aprilTagTypeSupplier + .get() + .getLayout() + .getTagPose(detectionEntry.getKey()) + .ifPresent(allTagPoses::add); } } Logger.recordOutput("AprilTagVision/TagPoses", allTagPoses.toArray(Pose3d[]::new)); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVisionIONorthstar.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVisionIONorthstar.java index c592ad59..4df522a0 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVisionIONorthstar.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVisionIONorthstar.java @@ -9,14 +9,15 @@ import static org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionConstants.*; -import com.fasterxml.jackson.core.JsonProcessingException; -import com.fasterxml.jackson.databind.ObjectMapper; import edu.wpi.first.networktables.DoubleArraySubscriber; import edu.wpi.first.networktables.IntegerSubscriber; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.PubSubOption; +import edu.wpi.first.networktables.StringPublisher; import edu.wpi.first.wpilibj.Timer; +import java.util.function.Supplier; import org.littletonrobotics.frc2024.FieldConstants; +import org.littletonrobotics.frc2024.FieldConstants.AprilTagLayoutType; import org.littletonrobotics.frc2024.util.Alert; public class AprilTagVisionIONorthstar implements AprilTagVisionIO { @@ -26,6 +27,10 @@ public class AprilTagVisionIONorthstar implements AprilTagVisionIO { private static final int cameraExposure = 10; private static final int cameraGain = 25; + private final Supplier aprilTagTypeSupplier; + private AprilTagLayoutType lastAprilTagType = null; + + private final StringPublisher tagLayoutPublisher; private final DoubleArraySubscriber observationSubscriber; private final DoubleArraySubscriber demoObservationSubscriber; private final IntegerSubscriber fpsSubscriber; @@ -34,9 +39,9 @@ public class AprilTagVisionIONorthstar implements AprilTagVisionIO { private final Alert disconnectedAlert; private final Timer disconnectedTimer = new Timer(); - public AprilTagVisionIONorthstar(int index) { + public AprilTagVisionIONorthstar(Supplier aprilTagTypeSupplier, int index) { + this.aprilTagTypeSupplier = aprilTagTypeSupplier; var northstarTable = NetworkTableInstance.getDefault().getTable(instanceNames[index]); - var configTable = northstarTable.getSubTable("config"); configTable.getStringTopic("camera_id").publish().set(cameraIds[index]); configTable.getIntegerTopic("camera_resolution_width").publish().set(cameraResolutionWidth); @@ -45,14 +50,7 @@ public AprilTagVisionIONorthstar(int index) { configTable.getIntegerTopic("camera_exposure").publish().set(cameraExposure); configTable.getIntegerTopic("camera_gain").publish().set(cameraGain); configTable.getDoubleTopic("fiducial_size_m").publish().set(FieldConstants.aprilTagWidth); - try { - configTable - .getStringTopic("tag_layout") - .publish() - .set(new ObjectMapper().writeValueAsString(FieldConstants.aprilTags)); - } catch (JsonProcessingException e) { - throw new RuntimeException("Failed to serialize AprilTag layout JSON for Northstar"); - } + tagLayoutPublisher = configTable.getStringTopic("tag_layout").publish(); var outputTable = northstarTable.getSubTable("output"); observationSubscriber = @@ -73,6 +71,14 @@ public AprilTagVisionIONorthstar(int index) { } public void updateInputs(AprilTagVisionIOInputs inputs) { + // Publish tag layout + var aprilTagType = aprilTagTypeSupplier.get(); + if (aprilTagType != lastAprilTagType) { + lastAprilTagType = aprilTagType; + tagLayoutPublisher.set(aprilTagType.getLayoutString()); + } + + // Get observations var queue = observationSubscriber.readQueue(); inputs.timestamps = new double[queue.length]; inputs.frames = new double[queue.length][]; diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/DriveConstants.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/DriveConstants.java index 145bb322..1fbb6db8 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/DriveConstants.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/DriveConstants.java @@ -26,7 +26,7 @@ public final class DriveConstants { switch (Constants.getRobot()) { case SIMBOT, COMPBOT -> DriveConfig.builder() - .wheelRadius(Units.inchesToMeters(2.026301746625535)) + .wheelRadius(Units.inchesToMeters(1.9231687266059994)) .trackWidthX(Units.inchesToMeters(20.75)) .trackWidthY(Units.inchesToMeters(20.75)) .bumperWidthX(Units.inchesToMeters(37)) diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/AutoAlignController.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/AutoAlignController.java index a10a5ef2..6ac8816f 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/AutoAlignController.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/AutoAlignController.java @@ -13,6 +13,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; import java.util.function.Supplier; import lombok.experimental.ExtensionMethod; import org.littletonrobotics.frc2024.RobotState; @@ -25,7 +26,7 @@ @ExtensionMethod({GeomUtil.class}) public class AutoAlignController { private static final LoggedTunableNumber linearkP = - new LoggedTunableNumber("AutoAlign/drivekP", 2.0); + new LoggedTunableNumber("AutoAlign/drivekP", 1.5); private static final LoggedTunableNumber linearkD = new LoggedTunableNumber("AutoAlign/drivekD", 0.0); private static final LoggedTunableNumber thetakP = @@ -33,9 +34,11 @@ public class AutoAlignController { private static final LoggedTunableNumber thetakD = new LoggedTunableNumber("AutoAlign/thetakD", 0.0); private static final LoggedTunableNumber linearTolerance = - new LoggedTunableNumber("AutoAlign/controllerLinearTolerance", 0.04); + new LoggedTunableNumber("AutoAlign/controllerLinearTolerance", 0.08); private static final LoggedTunableNumber thetaTolerance = - new LoggedTunableNumber("AutoAlign/controllerThetaTolerance", Units.degreesToRadians(3.0)); + new LoggedTunableNumber("AutoAlign/controllerThetaTolerance", Units.degreesToRadians(2.0)); + private static final LoggedTunableNumber toleranceTime = + new LoggedTunableNumber("AutoAlign/controllerToleranceSecs", 0.5); private static final LoggedTunableNumber maxLinearVelocity = new LoggedTunableNumber( "AutoAlign/maxLinearVelocity", DriveConstants.driveConfig.maxLinearVelocity()); @@ -70,6 +73,7 @@ public class AutoAlignController { // Controllers for translation and rotation private final ProfiledPIDController linearController; private final ProfiledPIDController thetaController; + private final Timer toleranceTimer = new Timer(); public AutoAlignController(Supplier poseSupplier, boolean slowMode) { this.poseSupplier = poseSupplier; @@ -84,6 +88,7 @@ public AutoAlignController(Supplier poseSupplier, boolean slowMode) { thetakP.get(), 0, thetakD.get(), new TrapezoidProfile.Constraints(0, 0)); thetaController.enableContinuousInput(-Math.PI, Math.PI); thetaController.setTolerance(thetaTolerance.get()); + toleranceTimer.restart(); updateConstraints(); resetControllers(); } @@ -183,6 +188,11 @@ public ChassisSpeeds update() { currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians()); if (thetaController.atGoal()) thetaVelocity = 0.0; + // Reset tolerance timer + if (!linearController.atGoal() || !thetaController.atGoal()) { + toleranceTimer.reset(); + } + // Log data Logger.recordOutput("AutoAlign/DistanceMeasured", currentDistance); Logger.recordOutput("AutoAlign/DistanceSetpoint", linearController.getSetpoint().position); @@ -207,6 +217,6 @@ public ChassisSpeeds update() { @AutoLogOutput(key = "AutoAlign/AtGoal") public boolean atGoal() { - return linearController.atGoal() && thetaController.atGoal(); + return toleranceTimer.hasElapsed(toleranceTime.get()); } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java index adb09670..4d3c21be 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java @@ -40,7 +40,7 @@ public static void main(String[] args) { .setVehicleLength(DriveConstants.driveConfig.trackWidthX()) .setVehicleWidth(DriveConstants.driveConfig.trackWidthY()) .setWheelRadius(DriveConstants.driveConfig.wheelRadius()) - .setMaxWheelTorque(4.2) + .setMaxWheelTorque(3.0) .setMaxWheelOmega( DriveConstants.moduleLimitsFree.maxDriveVelocity() / DriveConstants.driveConfig.wheelRadius() diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java index e2e612ee..8a27c06e 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java @@ -19,7 +19,7 @@ public class FlywheelConstants { public static final Gains gains = switch (Constants.getRobot()) { - case COMPBOT -> new Gains(0.25, 0, 0.0012, 0.8, 0.00105, 0); + case COMPBOT -> new Gains(0.25, 0, 0.0012, 0.50719, 0.00108, 0); case DEVBOT -> new Gains(0.0003, 0.0, 0.0, 0.33329, 0.00083, 0.0); case SIMBOT -> new Gains(0.05, 0.0, 0.0, 0.01, 0.00103, 0.0); }; diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java index 546529c7..10d4c3fa 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java @@ -43,6 +43,10 @@ public class Flywheels extends SubsystemBase { new LoggedTunableNumber("Flywheels/IntakingRpm", -3000.0); private static final LoggedTunableNumber ejectingRpm = new LoggedTunableNumber("Flywheels/EjectingRpm", 1000.0); + private static final LoggedTunableNumber poopingRpm = + new LoggedTunableNumber("Flywheels/PoopingRpm", 2000.0); + private static final LoggedTunableNumber superPoopRpm = + new LoggedTunableNumber("Flywheels/SuperPoopingRpm", 2500.0); private static final LoggedTunableNumber maxAcceleration = new LoggedTunableNumber( "Flywheels/MaxAccelerationRpmPerSec", flywheelConfig.maxAcclerationRpmPerSec()); @@ -69,6 +73,8 @@ public enum Goal { SHOOT(shootingLeftRpm, shootingRightRpm), INTAKE(intakingRpm, intakingRpm), EJECT(ejectingRpm, ejectingRpm), + POOP(poopingRpm, poopingRpm), + SUPER_POOP(superPoopRpm, superPoopRpm), CHARACTERIZING(() -> 0.0, () -> 0.0); private final DoubleSupplier leftGoal; @@ -219,8 +225,12 @@ public Command intakeCommand() { .withName("Flywheels Intake"); } - public Command ejectCommand() { - return startEnd(() -> setGoal(Goal.EJECT), () -> setGoal(Goal.IDLE)) - .withName("Flywheels Eject"); + public Command poopCommand() { + return startEnd(() -> setGoal(Goal.POOP), () -> setGoal(Goal.IDLE)).withName("Flywheels Poop"); + } + + public Command superPoopCommand() { + return startEnd(() -> setGoal(Goal.SUPER_POOP), () -> setGoal(Goal.IDLE)) + .withName("Flywheels Super Poop"); } } 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 458dc806..1eb7661e 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/leds/Leds.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/leds/Leds.java @@ -80,7 +80,6 @@ public static Leds getInstance() { private static final double autoFadeMaxTime = 5.0; // Return to normal private Leds() { - System.out.println("[Init] Creating LEDs"); leds = new AddressableLED(0); buffer = new AddressableLEDBuffer(length); leds.setLength(length); @@ -135,7 +134,10 @@ public synchronized void periodic() { if (estopped) { solid(Color.kRed); } else if (DriverStation.isDisabled()) { - if (lastEnabledAuto && Timer.getFPGATimestamp() - lastEnabledTime < autoFadeMaxTime) { + if (armCoast) { + // Arm coast alert + solid(Color.kWhite); + } else if (lastEnabledAuto && Timer.getFPGATimestamp() - lastEnabledTime < autoFadeMaxTime) { // Auto fade solid(1.0 - ((Timer.getFPGATimestamp() - lastEnabledTime) / autoFadeTime), Color.kGreen); } else if (lowBatteryAlert) { @@ -190,11 +192,6 @@ public synchronized void periodic() { } } - // Arm coast alert - if (armCoast) { - solid(Color.kWhite); - } - // Arm estop alert if (armEstopped) { solid(Color.kRed); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java index 1097c695..1cf9a76c 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java @@ -110,11 +110,13 @@ public void periodic() { switch (goal) { case IDLE -> {} case FLOOR_INTAKE -> { - feeder.setGoal(Feeder.Goal.FLOOR_INTAKING); - intake.setGoal(Intake.Goal.FLOOR_INTAKING); if (gamepieceState == GamepieceState.SHOOTER_STAGED) { + intake.setGoal(Intake.Goal.EJECTING); + feeder.setGoal(Feeder.Goal.EJECTING); indexer.setGoal(Indexer.Goal.IDLING); } else { + intake.setGoal(Intake.Goal.FLOOR_INTAKING); + feeder.setGoal(Feeder.Goal.FLOOR_INTAKING); indexer.setGoal(Indexer.Goal.FLOOR_INTAKING); } } @@ -136,7 +138,7 @@ public void periodic() { } case EJECT_TO_FLOOR -> { feeder.setGoal(Feeder.Goal.EJECTING); - indexer.setGoal(Indexer.Goal.EJECTING); + indexer.setGoal(Indexer.Goal.IDLING); intake.setGoal(Intake.Goal.EJECTING); backpack.setGoal(Backpack.Goal.IDLING); } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/GenericSlamElevator.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/GenericSlamElevator.java index ee7b14b5..67282c9f 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/GenericSlamElevator.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/GenericSlamElevator.java @@ -113,7 +113,8 @@ public void periodic() { staticTimer.reset(); } // If we are finished with timer, finish goal. - atGoal = staticTimer.hasElapsed(staticTimeSecs); + // Also assume we are at the goal if auto was started + atGoal = staticTimer.hasElapsed(staticTimeSecs) || DriverStation.isAutonomousEnabled(); } else { staticTimer.stop(); staticTimer.reset(); @@ -134,13 +135,17 @@ public void periodic() { // Reset io.stop(); lastGoal = null; - atGoal = false; staticTimer.stop(); staticTimer.reset(); - // Set to coast mode - setBrakeMode(!coastModeSupplier.getAsBoolean()); + if (Math.abs(inputs.velocityRadsPerSec) > minVelocityThresh) { + // If we don't move when disabled, assume we are still at goal + atGoal = false; + } } + // Update coast mode + setBrakeMode(!coastModeSupplier.getAsBoolean()); + Logger.recordOutput("Superstructure/" + name + "/Goal", getGoal().toString()); } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java index 79bc98f3..fe8d30e0 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java @@ -24,6 +24,7 @@ public enum Goal { STOW, BACKPACK_OUT_UNJAM, AIM, + SUPER_POOP, INTAKE, UNJAM_INTAKE, STATION_INTAKE, @@ -99,6 +100,11 @@ public void periodic() { climber.setGoal(Climber.Goal.IDLE); backpackActuator.setGoal(BackpackActuator.Goal.RETRACT); } + case SUPER_POOP -> { + arm.setGoal(Arm.Goal.SUPER_POOP); + climber.setGoal(Climber.Goal.IDLE); + backpackActuator.setGoal(BackpackActuator.Goal.RETRACT); + } case INTAKE -> { arm.setGoal(Arm.Goal.FLOOR_INTAKE); climber.setGoal(Climber.Goal.IDLE); @@ -120,8 +126,14 @@ public void periodic() { backpackActuator.setGoal(BackpackActuator.Goal.RETRACT); } case RESET_CLIMB -> { - arm.setGoal(Arm.Goal.STOP); - climber.setGoal(Climber.Goal.IDLE); + arm.setGoal(Arm.Goal.RESET_CLIMB); + if (arm.atGoal()) { + // Retract and then stop + climber.setGoal(Climber.Goal.IDLE); + } else { + // Arm in unsafe state to retract, apply no current + climber.setGoal(Climber.Goal.STOP); + } backpackActuator.setGoal(BackpackActuator.Goal.RETRACT); } case PREPARE_PREPARE_TRAP_CLIMB -> { 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 8c6dcc4c..de7d6c98 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 @@ -77,6 +77,7 @@ public enum Goal { UNJAM_INTAKE(new LoggedTunableNumber("Arm/UnjamDegrees", 40.0)), STATION_INTAKE(new LoggedTunableNumber("Arm/StationIntakeDegrees", 45.0)), AIM(() -> RobotState.getInstance().getAimingParameters().armAngle().getDegrees()), + SUPER_POOP(new LoggedTunableNumber("Arm/SuperPoopDegrees", 48.0)), STOW(new LoggedTunableNumber("Arm/StowDegrees", 0.0)), AMP(new LoggedTunableNumber("Arm/AmpDegrees", 110.0)), SUBWOOFER(new LoggedTunableNumber("Arm/SubwooferDegrees", 55.0)), @@ -84,6 +85,7 @@ public enum Goal { PREPARE_PREPARE_TRAP_CLIMB(new LoggedTunableNumber("Arm/PreparePrepareTrapClimbDegrees", 40.0)), PREPARE_CLIMB(new LoggedTunableNumber("Arm/PrepareClimbDegrees", 105.0)), CLIMB(new LoggedTunableNumber("Arm/ClimbDegrees", 90.0)), + RESET_CLIMB(new LoggedTunableNumber("Arm/ResetClimbDegrees", 30.0)), CUSTOM(new LoggedTunableNumber("Arm/CustomSetpoint", 20.0)); private final DoubleSupplier armSetpointSupplier; @@ -171,8 +173,8 @@ public void periodic() { Leds.getInstance().armEstopped = disableSupplier.getAsBoolean() && DriverStation.isEnabled(); // Set coast mode with override - setBrakeMode(!coastSupplier.getAsBoolean() || DriverStation.isEnabled()); - Leds.getInstance().armCoast = coastSupplier.getAsBoolean() && disableSupplier.getAsBoolean(); + setBrakeMode(!coastSupplier.getAsBoolean()); + Leds.getInstance().armCoast = coastSupplier.getAsBoolean(); // Don't run profile when characterizing, coast mode, or disabled if (!characterizing 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 aacf6709..35e146a5 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 @@ -60,7 +60,7 @@ public class ArmConstants { switch (Constants.getRobot()) { case SIMBOT -> new Gains(90.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); case DEVBOT -> new Gains(75.0, 0.0, 2.5, 0.0, 0.0, 0.0, 0.0); - case COMPBOT -> new Gains(50.0 * reduction, 0.0, 2.5 * reduction, 8.4, 0.0, 0.0, 22.9); + case COMPBOT -> new Gains(6000.0, 0.0, 250.0, 8.4, 0.0, 0.0, 22.9); }; public static TrapezoidProfile.Constraints profileConstraints = diff --git a/trajectory_native/Earthfile b/trajectory_native/Earthfile index 01f1d739..555fb418 100644 --- a/trajectory_native/Earthfile +++ b/trajectory_native/Earthfile @@ -1,25 +1,17 @@ VERSION 0.8 -FROM alpine:3.19.1 -WORKDIR /RobotCode2024/trajectory_native - -APK_ADD: - FUNCTION - ARG pkgs - ARG TARGETARCH - RUN mkdir -p /var/cache/apk - RUN ln -s /var/cache/apk /etc/apk/cache - RUN --mount=type=cache,id=apk-cache-$TARGETARCH,sharing=locked,target=/var/cache/apk apk add --update $pkgs - RUN rm -rf /var/cache/apk/* +FROM scratch apk-deps: - DO +APK_ADD --pkgs="grpc-dev protobuf-dev blas-dev lapack-dev" + FROM alpine:3.19.1 + WORKDIR /RobotCode2024/trajectory_native + RUN apk add --no-cache --update grpc-dev protobuf-dev blas-dev lapack-dev tinyxml2-dev SAVE IMAGE --push ghcr.io/mechanical-advantage/vts-apk-deps:latest ipopt: FROM +apk-deps ENV CC=clang ENV CXX=clang++ - DO +APK_ADD --pkgs="clang make patch gfortran" + RUN apk add --no-cache --update clang make patch gfortran RUN mkdir ipopt-mumps-build WORKDIR ipopt-mumps-build RUN wget -c https://github.com/coin-or-tools/ThirdParty-Mumps/archive/refs/tags/releases/3.0.5.tar.gz -O - | tar -xz @@ -40,29 +32,26 @@ ipopt: RUN ../configure RUN make -j8 RUN make DESTDIR=/RobotCode2024/trajectory_native/ipopt-mumps-build/installroot install - SAVE IMAGE --push ghcr.io/mechanical-advantage/vts-ipopt:latest SAVE ARTIFACT /RobotCode2024/trajectory_native/ipopt-mumps-build/installroot + SAVE IMAGE --push ghcr.io/mechanical-advantage/vts-ipopt:latest casadi: FROM +apk-deps - BUILD +ipopt COPY +ipopt/installroot/usr/local /usr/local/ RUN wget -c https://github.com/casadi/casadi/archive/refs/tags/3.6.4.tar.gz -O - | tar -xz RUN mkdir casadi-3.6.4/build WORKDIR casadi-3.6.4/build - DO +APK_ADD --pkgs="clang make cmake tinyxml2-dev" + RUN apk add --no-cache --update clang make cmake ENV CC=clang ENV CXX=clang++ RUN cmake -DWITH_IPOPT=ON -DWITH_DEEPBIND=OFF -DWITH_BUILD_TINYXML=OFF .. RUN make -j8 RUN make DESTDIR=$(pwd)/installroot install - SAVE IMAGE --push ghcr.io/mechanical-advantage/vts-casadi:latest SAVE ARTIFACT installroot + SAVE IMAGE --push ghcr.io/mechanical-advantage/vts-casadi:latest trajoptlib: FROM +apk-deps - BUILD +ipopt - BUILD +casadi COPY +ipopt/installroot/usr/local /usr/local/ COPY +casadi/installroot/usr/local /usr/local/ LET TRAJOPT_COMMIT=f6cf3d42359f6f41f311f848a4e7f51c3f88c2ca @@ -72,24 +61,21 @@ trajoptlib: # Use our CMakeLists.txt (uses system casadi install) RUN rm ../CMakeLists.txt COPY trajoptlib-CMakeLists.txt ../CMakeLists.txt - DO +APK_ADD --pkgs="clang make cmake git tinyxml2-dev" + RUN apk add --no-cache --update clang make cmake git ENV CC=clang ENV CXX=clang++ RUN cmake -DOPTIMIZER_BACKEND=casadi .. RUN make -j8 RUN make DESTDIR=$(pwd)/installroot install - SAVE IMAGE --push ghcr.io/mechanical-advantage/vts-trajoptlib:latest SAVE ARTIFACT installroot + SAVE IMAGE --push ghcr.io/mechanical-advantage/vts-trajoptlib:latest dev-image: FROM +apk-deps - BUILD +ipopt - BUILD +casadi - BUILD +trajoptlib COPY +ipopt/installroot/usr/local /usr/local/ COPY +casadi/installroot/usr/local /usr/local/ COPY +trajoptlib/installroot/usr/local /usr/local/ - DO +APK_ADD --pkgs="clang make cmake git gdb tinyxml2-dev nlohmann-json fmt-dev" + RUN apk add --no-cache --update clang make cmake git gdb nlohmann-json fmt-dev ENV CC=clang ENV CXX=clang++ SAVE IMAGE --push ghcr.io/mechanical-advantage/vts-dev-image:latest