From d946f7ebfadebe0d3816c3927be35444f8bf6834 Mon Sep 17 00:00:00 2001 From: Cameron Earle Date: Fri, 8 Mar 2024 00:47:38 -0500 Subject: [PATCH 01/34] Remove APK cached packages for VTS build --- trajectory_native/Earthfile | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/trajectory_native/Earthfile b/trajectory_native/Earthfile index 01f1d739..21fbd485 100644 --- a/trajectory_native/Earthfile +++ b/trajectory_native/Earthfile @@ -5,14 +5,10 @@ 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/* + RUN apk add --no-cache --update $pkgs apk-deps: - DO +APK_ADD --pkgs="grpc-dev protobuf-dev blas-dev lapack-dev" + DO +APK_ADD --pkgs="grpc-dev protobuf-dev blas-dev lapack-dev tinyxml2-dev" SAVE IMAGE --push ghcr.io/mechanical-advantage/vts-apk-deps:latest ipopt: @@ -50,7 +46,7 @@ casadi: 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" + DO +APK_ADD --pkgs="clang make cmake" ENV CC=clang ENV CXX=clang++ RUN cmake -DWITH_IPOPT=ON -DWITH_DEEPBIND=OFF -DWITH_BUILD_TINYXML=OFF .. @@ -72,7 +68,7 @@ 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" + DO +APK_ADD --pkgs="clang make cmake git" ENV CC=clang ENV CXX=clang++ RUN cmake -DOPTIMIZER_BACKEND=casadi .. @@ -89,7 +85,7 @@ dev-image: 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" + DO +APK_ADD --pkgs="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 From 7d3789b8f8f2746e824b62e300a90c74fcd79088 Mon Sep 17 00:00:00 2001 From: Cameron Earle Date: Fri, 8 Mar 2024 02:47:59 -0500 Subject: [PATCH 02/34] Simplify VTS Earthfile to fix caching --- trajectory_native/Earthfile | 32 +++++++++++--------------------- 1 file changed, 11 insertions(+), 21 deletions(-) diff --git a/trajectory_native/Earthfile b/trajectory_native/Earthfile index 21fbd485..555fb418 100644 --- a/trajectory_native/Earthfile +++ b/trajectory_native/Earthfile @@ -1,21 +1,17 @@ VERSION 0.8 -FROM alpine:3.19.1 -WORKDIR /RobotCode2024/trajectory_native - -APK_ADD: - FUNCTION - ARG pkgs - RUN apk add --no-cache --update $pkgs +FROM scratch apk-deps: - DO +APK_ADD --pkgs="grpc-dev protobuf-dev blas-dev lapack-dev tinyxml2-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 @@ -36,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" + 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 @@ -68,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" + 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 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 From 6c041e3d867dd00c4fce1906ed41d47f0a25436e Mon Sep 17 00:00:00 2001 From: Surya Thoppae <63197592+suryatho@users.noreply.github.com> Date: Fri, 8 Mar 2024 14:36:05 -0500 Subject: [PATCH 03/34] Use coast override for backpack actuator (#151) * Use coast override for backpack actuator Use generic name for coast override * Fix override name --------- Co-authored-by: Jonah <47046556+jwbonner@users.noreply.github.com> --- src/main/java/org/littletonrobotics/frc2024/RobotContainer.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 1fea6bd4..3b39c130 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -260,6 +260,7 @@ public RobotContainer() { arm.setOverrides(armDisable, armCoast); RobotState.getInstance().setLookaheadDisable(lookaheadDisable); climber.setCoastOverride(armCoast); + backpackActuator.setCoastOverride(armCoast); flywheels.setPrepareShootSupplier( () -> { return DriverStation.isTeleopEnabled() From 8322d86c5cc4452d37d2ac0127b4bba1e45a0de0 Mon Sep 17 00:00:00 2001 From: Surya Thoppae <63197592+suryatho@users.noreply.github.com> Date: Fri, 8 Mar 2024 14:36:39 -0500 Subject: [PATCH 04/34] Double press left bumper to start climb sequence (#152) * Double press left bumper to start climb sequence * check style --- src/main/java/org/littletonrobotics/frc2024/RobotContainer.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 3b39c130..97ba7892 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -585,6 +585,7 @@ private void configureButtonBindings() { operator.rightStick().onTrue(Commands.runOnce(() -> trapScoreMode = !trapScoreMode)); operator .leftBumper() + .doublePress() .and(() -> trapScoreMode) .toggleOnTrue( ClimbingCommands.climbNTrapSequence( @@ -600,6 +601,7 @@ private void configureButtonBindings() { .withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); operator .leftBumper() + .doublePress() .and(() -> !trapScoreMode) .toggleOnTrue( ClimbingCommands.simpleClimbSequence( From 52483de4dfcf9797210ad481de9eda875a24a6b0 Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Fri, 8 Mar 2024 14:38:39 -0500 Subject: [PATCH 05/34] Add overrides for simple AprilTag layouts (#175) * Add overrides for simple AprilTag layouts * Bug fixes --- src/main/deploy/apriltags/2024-amps.json | 44 +++ src/main/deploy/apriltags/2024-official.json | 296 ++++++++++++++++++ src/main/deploy/apriltags/2024-speakers.json | 80 +++++ .../{2024-wpi-custom.json => 2024-wpi.json} | 0 .../littletonrobotics/frc2024/Constants.java | 31 -- .../frc2024/FieldConstants.java | 43 ++- .../org/littletonrobotics/frc2024/Robot.java | 1 + .../frc2024/RobotContainer.java | 49 ++- .../apriltagvision/AprilTagVision.java | 25 +- .../AprilTagVisionIONorthstar.java | 30 +- 10 files changed, 531 insertions(+), 68 deletions(-) create mode 100644 src/main/deploy/apriltags/2024-amps.json create mode 100644 src/main/deploy/apriltags/2024-official.json create mode 100644 src/main/deploy/apriltags/2024-speakers.json rename src/main/deploy/apriltags/{2024-wpi-custom.json => 2024-wpi.json} (100%) 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..4ff13684 100644 --- a/src/main/java/org/littletonrobotics/frc2024/Robot.java +++ b/src/main/java/org/littletonrobotics/frc2024/Robot.java @@ -234,6 +234,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 97ba7892..f5b46312 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -26,6 +26,7 @@ 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 +101,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 = @@ -122,6 +126,17 @@ public class RobotContainer { 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 +162,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 +187,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 +234,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) { @@ -284,9 +306,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); } @@ -685,6 +704,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/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][]; From 24a5ec596ed986114fc17fc9619cb0fb75df2bd6 Mon Sep 17 00:00:00 2001 From: Cameron Earle Date: Fri, 8 Mar 2024 14:47:23 -0500 Subject: [PATCH 06/34] Only run VTS action when files are updated --- .github/workflows/vts.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/vts.yml b/.github/workflows/vts.yml index e956ddea..ccb3ecf2 100644 --- a/.github/workflows/vts.yml +++ b/.github/workflows/vts.yml @@ -2,7 +2,8 @@ name: VTS Build on: push: - pull_request: + paths: + - 'trajectory_native/**' concurrency: group: ${{ github.workflow }}-${{ github.ref }} From 468fbf5ebda21540c0abb66a54d5290274a2c65c Mon Sep 17 00:00:00 2001 From: Surya Thoppae <63197592+suryatho@users.noreply.github.com> Date: Fri, 8 Mar 2024 14:59:04 -0500 Subject: [PATCH 07/34] Only allow coast when override is switched during disable (#162) * Only allow coast when override is switched during disable * Rename * Fixes --------- Co-authored-by: Jonah <47046556+jwbonner@users.noreply.github.com> --- .../frc2024/RobotContainer.java | 20 ++++++++++++++++--- .../frc2024/subsystems/leds/Leds.java | 10 ++++------ .../superstructure/GenericSlamElevator.java | 5 +++-- .../subsystems/superstructure/arm/Arm.java | 4 ++-- 4 files changed, 26 insertions(+), 13 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index f5b46312..3350b234 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -22,6 +22,7 @@ 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; @@ -121,6 +122,7 @@ public class RobotContainer { private boolean podiumShotMode = false; private boolean trapScoreMode = true; + private boolean armCoastOverride = false; // Dashboard inputs private final LoggedDashboardChooser autoChooser = @@ -279,10 +281,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); - backpackActuator.setCoastOverride(armCoast); flywheels.setPrepareShootSupplier( () -> { return DriverStation.isTeleopEnabled() 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..89cf42f7 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/leds/Leds.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/leds/Leds.java @@ -135,7 +135,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 +193,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/superstructure/GenericSlamElevator.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/GenericSlamElevator.java index ee7b14b5..9f78bfc2 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/GenericSlamElevator.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/GenericSlamElevator.java @@ -137,10 +137,11 @@ public void periodic() { atGoal = false; staticTimer.stop(); staticTimer.reset(); - // Set to coast mode - setBrakeMode(!coastModeSupplier.getAsBoolean()); } + // Update coast mode + setBrakeMode(!coastModeSupplier.getAsBoolean()); + Logger.recordOutput("Superstructure/" + name + "/Goal", getGoal().toString()); } 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 955f27b7..a5fe686c 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 @@ -170,8 +170,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 From 14346fd3d98722ffa7347027a7e7067652e14842 Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Fri, 8 Mar 2024 15:15:15 -0500 Subject: [PATCH 08/34] Remove init messages --- src/main/java/org/littletonrobotics/frc2024/Robot.java | 2 -- .../org/littletonrobotics/frc2024/subsystems/leds/Leds.java | 1 - 2 files changed, 3 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/Robot.java b/src/main/java/org/littletonrobotics/frc2024/Robot.java index 4ff13684..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); 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 89cf42f7..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); From 98fff14a06b21f0b7862539b82f6fb8d5d6b9e0f Mon Sep 17 00:00:00 2001 From: Surya Thoppae <63197592+suryatho@users.noreply.github.com> Date: Fri, 8 Mar 2024 15:15:45 -0500 Subject: [PATCH 09/34] Stop intake motor when shooter is staged while intaking (#165) * Stop intake motor when shooter staged * Better intake & eject --------- Co-authored-by: Jonah <47046556+jwbonner@users.noreply.github.com> --- .../frc2024/subsystems/rollers/Rollers.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) 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..a0b1402e 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java @@ -110,12 +110,14 @@ 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) { + feeder.setGoal(Feeder.Goal.IDLING); indexer.setGoal(Indexer.Goal.IDLING); + intake.setGoal(Intake.Goal.IDLING); } else { + feeder.setGoal(Feeder.Goal.FLOOR_INTAKING); indexer.setGoal(Indexer.Goal.FLOOR_INTAKING); + intake.setGoal(Intake.Goal.FLOOR_INTAKING); } } case UNTACO -> { @@ -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); } From ba1826b9039dfa304244036c6eecca3a656f3379 Mon Sep 17 00:00:00 2001 From: Surya Thoppae <63197592+suryatho@users.noreply.github.com> Date: Fri, 8 Mar 2024 15:27:49 -0500 Subject: [PATCH 10/34] Poop. (#167) * Add small pooping mode for flywheels * Auto eject after poop --------- Co-authored-by: Jonah <47046556+jwbonner@users.noreply.github.com> --- .../org/littletonrobotics/frc2024/RobotContainer.java | 10 ++++++++++ .../frc2024/subsystems/flywheels/Flywheels.java | 8 +++++--- 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 3350b234..c21435fa 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -478,6 +478,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 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..fb151884 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,8 @@ 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 maxAcceleration = new LoggedTunableNumber( "Flywheels/MaxAccelerationRpmPerSec", flywheelConfig.maxAcclerationRpmPerSec()); @@ -69,6 +71,7 @@ public enum Goal { SHOOT(shootingLeftRpm, shootingRightRpm), INTAKE(intakingRpm, intakingRpm), EJECT(ejectingRpm, ejectingRpm), + POOP(poopingRpm, poopingRpm), CHARACTERIZING(() -> 0.0, () -> 0.0); private final DoubleSupplier leftGoal; @@ -219,8 +222,7 @@ 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.POOP)).withName("Flywheels Poop"); } } From a3bf9738fb2d711882315706c9e01738ddccc6a9 Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Fri, 8 Mar 2024 16:08:34 -0500 Subject: [PATCH 11/34] Add super poop (#180) --- .../frc2024/RobotContainer.java | 16 ++++++++++++++++ .../frc2024/subsystems/flywheels/Flywheels.java | 10 +++++++++- .../superstructure/Superstructure.java | 6 ++++++ .../subsystems/superstructure/arm/Arm.java | 1 + 4 files changed, 32 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index c21435fa..74222094 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -445,13 +445,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()); 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 fb151884..10d4c3fa 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java @@ -45,6 +45,8 @@ public class Flywheels extends SubsystemBase { 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()); @@ -72,6 +74,7 @@ public enum Goal { INTAKE(intakingRpm, intakingRpm), EJECT(ejectingRpm, ejectingRpm), POOP(poopingRpm, poopingRpm), + SUPER_POOP(superPoopRpm, superPoopRpm), CHARACTERIZING(() -> 0.0, () -> 0.0); private final DoubleSupplier leftGoal; @@ -223,6 +226,11 @@ public Command intakeCommand() { } public Command poopCommand() { - return startEnd(() -> setGoal(Goal.POOP), () -> setGoal(Goal.POOP)).withName("Flywheels Poop"); + 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/superstructure/Superstructure.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java index d0859260..5c7003a8 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, @@ -98,6 +99,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); 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 a5fe686c..d87bdd90 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)), From a335823e983760107228d907c40412ebe164f984 Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Fri, 8 Mar 2024 16:33:25 -0500 Subject: [PATCH 12/34] Improve climber retract behavior (#169) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Improve climber retract behavior Don't pull on wires 😬 * Fix formatting * Disable climber in auto --- .../superstructure/GenericSlamElevator.java | 5 ++++- .../subsystems/superstructure/Superstructure.java | 14 ++++++++++++-- .../frc2024/subsystems/superstructure/arm/Arm.java | 1 + 3 files changed, 17 insertions(+), 3 deletions(-) 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 9f78bfc2..2bfce5cb 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/GenericSlamElevator.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/GenericSlamElevator.java @@ -134,9 +134,12 @@ public void periodic() { // Reset io.stop(); lastGoal = null; - atGoal = false; staticTimer.stop(); staticTimer.reset(); + if (Math.abs(inputs.velocityRadsPerSec) > minVelocityThresh) { + // If we don't move when disabled, assume we are still at goal + atGoal = false; + } } // Update coast mode 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 5c7003a8..1329db15 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java @@ -125,8 +125,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_CLIMB -> { @@ -164,6 +170,10 @@ public void periodic() { backpackActuator.setGoal(BackpackActuator.Goal.RETRACT); } } + if (DriverStation.isAutonomousEnabled()) { + // Climber reset sequence disabled in auto, don't try to move while at an unsafe arm angle + climber.setGoal(Climber.Goal.STOP); + } arm.periodic(); climber.periodic(); 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 d87bdd90..a319f4fc 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 @@ -84,6 +84,7 @@ public enum Goal { PODIUM(new LoggedTunableNumber("Arm/PodiumDegrees", 30.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; From bb51ff72659de0798ec5ca5585a71f59d349aef7 Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Fri, 8 Mar 2024 16:37:53 -0500 Subject: [PATCH 13/34] Update wheel radius --- .../frc2024/subsystems/drive/DriveConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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)) From edee4eaf5e5cc6a71906eb7c89a3e3fe529ab3e8 Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Fri, 8 Mar 2024 16:47:18 -0500 Subject: [PATCH 14/34] Retune compbot arm --- .../frc2024/subsystems/superstructure/arm/ArmConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 = From 38900b7371dd4b7794b70f71560e48e67f22b1e1 Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Fri, 8 Mar 2024 16:52:06 -0500 Subject: [PATCH 15/34] Update flywheels FF --- .../frc2024/subsystems/flywheels/FlywheelConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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); }; From 949067ee8348a444953f1747e98f3194730100a9 Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Fri, 8 Mar 2024 17:35:21 -0500 Subject: [PATCH 16/34] Improve climber reset behavior after auto --- .../subsystems/superstructure/GenericSlamElevator.java | 3 ++- .../frc2024/subsystems/superstructure/Superstructure.java | 4 ---- 2 files changed, 2 insertions(+), 5 deletions(-) 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 2bfce5cb..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(); 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 1329db15..58ab6dc5 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java @@ -170,10 +170,6 @@ public void periodic() { backpackActuator.setGoal(BackpackActuator.Goal.RETRACT); } } - if (DriverStation.isAutonomousEnabled()) { - // Climber reset sequence disabled in auto, don't try to move while at an unsafe arm angle - climber.setGoal(Climber.Goal.STOP); - } arm.periodic(); climber.periodic(); From 6f559cf4c30d2608842c4afc750a84d67a0617d1 Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Fri, 8 Mar 2024 17:44:35 -0500 Subject: [PATCH 17/34] Tune angle map --- .../littletonrobotics/frc2024/RobotState.java | 35 ++++++++----------- 1 file changed, 14 insertions(+), 21 deletions(-) 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; From 2e0e6f72ce49834eab63ace52935e3724e018cce Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Fri, 8 Mar 2024 18:47:39 -0500 Subject: [PATCH 18/34] Adjust auto align tolerance & wait for time (#172) * Adjust auto align tolerance & wait for time * Tune amp auto align --- .../frc2024/RobotContainer.java | 15 ++++++++++----- .../drive/controllers/AutoAlignController.java | 18 ++++++++++++++---- 2 files changed, 24 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 74222094..f29009e0 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; @@ -543,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() 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..d2a18b9e 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.05); 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.4); 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()); } } From f94d067cba1bf3b1f1bfc820c36acda2e8e8e54d Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Fri, 8 Mar 2024 19:26:38 -0500 Subject: [PATCH 19/34] Lower max drive torque in auto --- .../subsystems/drive/trajectory/GenerateTrajectories.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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() From a8d0afdf6df1240cd34f56d1f1a78ece0a61cb4d Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Fri, 8 Mar 2024 20:29:15 -0500 Subject: [PATCH 20/34] Eject from intake after first note is staged --- .../frc2024/subsystems/rollers/Rollers.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 a0b1402e..1cf9a76c 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java @@ -111,13 +111,13 @@ public void periodic() { case IDLE -> {} case FLOOR_INTAKE -> { if (gamepieceState == GamepieceState.SHOOTER_STAGED) { - feeder.setGoal(Feeder.Goal.IDLING); + intake.setGoal(Intake.Goal.EJECTING); + feeder.setGoal(Feeder.Goal.EJECTING); indexer.setGoal(Indexer.Goal.IDLING); - intake.setGoal(Intake.Goal.IDLING); } else { + intake.setGoal(Intake.Goal.FLOOR_INTAKING); feeder.setGoal(Feeder.Goal.FLOOR_INTAKING); indexer.setGoal(Indexer.Goal.FLOOR_INTAKING); - intake.setGoal(Intake.Goal.FLOOR_INTAKING); } } case UNTACO -> { From 817763d367b90c18ba967afef6dd2abc5d06b5c3 Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Fri, 8 Mar 2024 20:29:26 -0500 Subject: [PATCH 21/34] Increase auto align tolerance --- .../subsystems/drive/controllers/AutoAlignController.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 d2a18b9e..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 @@ -34,11 +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.05); + new LoggedTunableNumber("AutoAlign/controllerLinearTolerance", 0.08); private static final LoggedTunableNumber thetaTolerance = new LoggedTunableNumber("AutoAlign/controllerThetaTolerance", Units.degreesToRadians(2.0)); private static final LoggedTunableNumber toleranceTime = - new LoggedTunableNumber("AutoAlign/controllerToleranceSecs", 0.4); + new LoggedTunableNumber("AutoAlign/controllerToleranceSecs", 0.5); private static final LoggedTunableNumber maxLinearVelocity = new LoggedTunableNumber( "AutoAlign/maxLinearVelocity", DriveConstants.driveConfig.maxLinearVelocity()); From b85fb55afe3f988dd40091c01fd85c2e5313b35d Mon Sep 17 00:00:00 2001 From: Surya Thoppae <63197592+suryatho@users.noreply.github.com> Date: Fri, 8 Mar 2024 21:16:57 -0500 Subject: [PATCH 22/34] Raise arm to halfway prepare prepare setpoint with driver x (#158) * Raise arm to halfway prepare prepare setpoint with driver x * Driver should toggle arm halfway up * Fix this --------- Co-authored-by: Cameron Earle --- .../frc2024/RobotContainer.java | 17 ++++------------- .../frc2024/commands/ClimbingCommands.java | 2 +- .../subsystems/rollers/backpack/Backpack.java | 2 +- .../superstructure/Superstructure.java | 7 +++++++ .../subsystems/superstructure/arm/Arm.java | 1 + 5 files changed, 14 insertions(+), 15 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index f29009e0..12763962 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -310,6 +310,7 @@ public RobotContainer() { < Units.feetToMeters(25.0) && rollers.getGamepieceState() == GamepieceState.SHOOTER_STAGED && superstructure.getCurrentGoal() != Superstructure.Goal.PREPARE_CLIMB + && superstructure.getCurrentGoal() != Superstructure.Goal.PREPARE_PREPARE_TRAP_CLIMB && superstructure.getCurrentGoal() != Superstructure.Goal.CLIMB && superstructure.getCurrentGoal() != Superstructure.Goal.TRAP && superstructure.getCurrentGoal() != Superstructure.Goal.CANCEL_PREPARE_CLIMB @@ -609,20 +610,10 @@ private void configureButtonBindings() { () -> superstructure.getCurrentGoal() != Superstructure.Goal.CANCEL_CLIMB && superstructure.getCurrentGoal() != Superstructure.Goal.CANCEL_PREPARE_CLIMB) - .whileTrue( + .toggleOnTrue( Commands.either( - ClimbingCommands.autoDrive( - false, - drive, - () -> -driver.getLeftY(), - () -> -driver.getLeftX(), - autoDriveDisable), - ClimbingCommands.autoDrive( - true, - drive, - () -> -driver.getLeftY(), - () -> -driver.getLeftX(), - autoDriveDisable), + superstructure.setGoalCommand(Superstructure.Goal.PREPARE_PREPARE_TRAP_CLIMB), + Commands.none(), // No function yet () -> trapScoreMode)); // ------------- Operator Controls ------------- diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java b/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java index db59e2bf..ee25504f 100644 --- a/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java +++ b/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java @@ -34,7 +34,7 @@ public class ClimbingCommands { private static final LoggedTunableNumber climbedXOffset = new LoggedTunableNumber("ClimbingCommands/ClimbedXOffset", 0.16); private static final LoggedTunableNumber chainToBack = - new LoggedTunableNumber("ClimbingCommands/ChainToBackOffset", 0.65); + new LoggedTunableNumber("ClimbingCommands/ChainToBackOffset", 0.3); private static final LoggedTunableNumber chainToFront = new LoggedTunableNumber("ClimbingCommands/ChainToFrontOffset", 0.9); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/Backpack.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/Backpack.java index d28b0757..1a36c7fd 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/Backpack.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/Backpack.java @@ -22,7 +22,7 @@ public class Backpack extends GenericRollerSystem { public enum Goal implements VoltageGoal { IDLING(() -> 0), AMP_SCORING(new LoggedTunableNumber("Backpack/AmpScoringVoltage", 12.0)), - TRAP_SCORING(new LoggedTunableNumber("Backpack/TrapScoringVoltage", 2.0)), + TRAP_SCORING(new LoggedTunableNumber("Backpack/TrapScoringVoltage", 8.0)), EJECTING(new LoggedTunableNumber("Backpack/EjectingVoltage", -12.0)); private final DoubleSupplier voltageSupplier; 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 58ab6dc5..b65b57b2 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java @@ -32,6 +32,7 @@ public enum Goal { SUBWOOFER, PODIUM, RESET_CLIMB, + PREPARE_PREPARE_TRAP_CLIMB, PREPARE_CLIMB, CANCEL_PREPARE_CLIMB, CLIMB, @@ -65,6 +66,7 @@ public void periodic() { // Retract climber if (!climber.retracted() + && desiredGoal != Goal.PREPARE_PREPARE_TRAP_CLIMB && desiredGoal != Goal.PREPARE_CLIMB && desiredGoal != Goal.CLIMB && desiredGoal != Goal.TRAP @@ -135,6 +137,11 @@ public void periodic() { } backpackActuator.setGoal(BackpackActuator.Goal.RETRACT); } + case PREPARE_PREPARE_TRAP_CLIMB -> { + arm.setGoal(Arm.Goal.PREPARE_PREPARE_TRAP_CLIMB); + climber.setGoal(Climber.Goal.EXTEND); + backpackActuator.setGoal(BackpackActuator.Goal.RETRACT); + } case PREPARE_CLIMB -> { arm.setGoal(Arm.Goal.PREPARE_CLIMB); climber.setGoal(Climber.Goal.EXTEND); 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 a319f4fc..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 @@ -82,6 +82,7 @@ public enum Goal { AMP(new LoggedTunableNumber("Arm/AmpDegrees", 110.0)), SUBWOOFER(new LoggedTunableNumber("Arm/SubwooferDegrees", 55.0)), PODIUM(new LoggedTunableNumber("Arm/PodiumDegrees", 30.0)), + 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)), From 45ec710defcc89baeac386c1e2da5cc6b3ef0b04 Mon Sep 17 00:00:00 2001 From: Cameron Earle Date: Sat, 9 Mar 2024 02:20:29 -0500 Subject: [PATCH 23/34] Remove ENV from VTS Earthfile --- trajectory_native/Earthfile | 44 ++++++++++++++----------------------- 1 file changed, 17 insertions(+), 27 deletions(-) diff --git a/trajectory_native/Earthfile b/trajectory_native/Earthfile index 555fb418..77b9c0b6 100644 --- a/trajectory_native/Earthfile +++ b/trajectory_native/Earthfile @@ -9,29 +9,25 @@ apk-deps: ipopt: FROM +apk-deps - ENV CC=clang - ENV CXX=clang++ 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 WORKDIR ThirdParty-Mumps-releases-3.0.5 RUN ./get.Mumps - RUN ./configure - RUN make -j8 + RUN CC=clang CXX=clang++ ./configure + RUN CC=clang CXX=clang++ make -j8 # Install to both system (for subsequent ipopt build) and installroot (to copy out as artifacts) - RUN make install - RUN make DESTDIR=/RobotCode2024/trajectory_native/ipopt-mumps-build/installroot install + RUN CC=clang CXX=clang++ make install + RUN CC=clang CXX=clang++ make DESTDIR=/RobotCode2024/trajectory_native/ipopt-mumps-build/installroot install WORKDIR .. RUN wget -c https://github.com/coin-or/Ipopt/archive/refs/tags/releases/3.14.14.tar.gz -O - | tar -xz RUN mkdir Ipopt-releases-3.14.14/build WORKDIR Ipopt-releases-3.14.14/build - ENV CC=clang - ENV CXX=clang++ - RUN ../configure - RUN make -j8 - RUN make DESTDIR=/RobotCode2024/trajectory_native/ipopt-mumps-build/installroot install + RUN CC=clang CXX=clang++ ../configure + RUN CC=clang CXX=clang++ make -j8 + RUN CC=clang CXX=clang++ make DESTDIR=/RobotCode2024/trajectory_native/ipopt-mumps-build/installroot install SAVE ARTIFACT /RobotCode2024/trajectory_native/ipopt-mumps-build/installroot SAVE IMAGE --push ghcr.io/mechanical-advantage/vts-ipopt:latest @@ -42,11 +38,9 @@ casadi: RUN mkdir casadi-3.6.4/build WORKDIR casadi-3.6.4/build 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 + RUN CC=clang CXX=clang++ cmake -DWITH_IPOPT=ON -DWITH_DEEPBIND=OFF -DWITH_BUILD_TINYXML=OFF .. + RUN CC=clang CXX=clang++ make -j8 + RUN CC=clang CXX=clang++ make DESTDIR=$(pwd)/installroot install SAVE ARTIFACT installroot SAVE IMAGE --push ghcr.io/mechanical-advantage/vts-casadi:latest @@ -62,11 +56,9 @@ trajoptlib: RUN rm ../CMakeLists.txt COPY trajoptlib-CMakeLists.txt ../CMakeLists.txt 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 + RUN CC=clang CXX=clang++ cmake -DOPTIMIZER_BACKEND=casadi .. + RUN CC=clang CXX=clang++ make -j8 + RUN CC=clang CXX=clang++ make DESTDIR=$(pwd)/installroot install SAVE ARTIFACT installroot SAVE IMAGE --push ghcr.io/mechanical-advantage/vts-trajoptlib:latest @@ -76,8 +68,8 @@ dev-image: COPY +casadi/installroot/usr/local /usr/local/ COPY +trajoptlib/installroot/usr/local /usr/local/ RUN apk add --no-cache --update clang make cmake git gdb nlohmann-json fmt-dev - ENV CC=clang - ENV CXX=clang++ + ENV CC="clang" + ENV CXX="clang++" SAVE IMAGE --push ghcr.io/mechanical-advantage/vts-dev-image:latest vts: @@ -87,10 +79,8 @@ vts: COPY CMakeLists.txt CMakeLists.txt RUN mkdir build WORKDIR build - ENV CC=clang - ENV CXX=clang++ - RUN cmake .. - RUN make -j8 + RUN CC=clang CXX=clang++ cmake .. + RUN CC=clang CXX=clang++ make -j8 EXPOSE 56328 ENV GRPC_VERBOSITY=info ENTRYPOINT ["./trajectory_native"] From 9e181199631f1775dda28afd0c995ccd51cf9a2b Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Sat, 9 Mar 2024 12:35:15 -0500 Subject: [PATCH 24/34] Clean up init --- .../org/littletonrobotics/frc2024/Robot.java | 12 +----- .../frc2024/RobotContainer.java | 41 ++++++++----------- 2 files changed, 18 insertions(+), 35 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/Robot.java b/src/main/java/org/littletonrobotics/frc2024/Robot.java index ba09d3e6..988098c3 100644 --- a/src/main/java/org/littletonrobotics/frc2024/Robot.java +++ b/src/main/java/org/littletonrobotics/frc2024/Robot.java @@ -10,12 +10,10 @@ import static org.littletonrobotics.frc2024.util.Alert.AlertType; import com.ctre.phoenix6.CANBus; -import edu.wpi.first.hal.AllianceStationID; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Threads; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.simulation.DriverStationSim; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import java.io.File; @@ -126,7 +124,6 @@ public void robotInit() { // Start AdvantageKit logger Logger.start(); - Leds.getInstance(); // Log active commands Map commandCounts = new HashMap<>(); @@ -155,11 +152,7 @@ public void robotInit() { logCommandFunction.accept(command, false); }); - // Default to blue alliance in sim - if (Constants.getMode() == Constants.Mode.SIM) { - DriverStationSim.setAllianceStationId(AllianceStationID.Blue1); - } - + // Reset alert timers canErrorTimer.restart(); canInitialErrorTimer.restart(); canivoreErrorTimer.restart(); @@ -190,9 +183,6 @@ public void robotInit() { } RobotController.setBrownoutVoltage(6.0); - - // Instantiate our RobotContainer. This will perform all our button bindings, - // and put our autonomous chooser on the dashboard. robotContainer = new RobotContainer(); } diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 12763962..707832f7 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -85,7 +85,7 @@ */ @ExtensionMethod({DoublePressTracker.class}) public class RobotContainer { - // Load robot state + // Load static objects private final RobotState robotState = RobotState.getInstance(); private final Leds leds = Leds.getInstance(); @@ -297,11 +297,11 @@ public RobotContainer() { arm.setOverrides(armDisable, () -> armCoastOverride); climber.setCoastOverride(() -> armCoastOverride); backpackActuator.setCoastOverride(() -> armCoastOverride); - RobotState.getInstance().setLookaheadDisable(lookaheadDisable); + robotState.setLookaheadDisable(lookaheadDisable); flywheels.setPrepareShootSupplier( () -> { return DriverStation.isTeleopEnabled() - && RobotState.getInstance() + && robotState .getEstimatedPose() .getTranslation() .getDistance( @@ -334,14 +334,14 @@ public RobotContainer() { () -> { driver.getHID().setRumble(RumbleType.kBothRumble, 1.0); operator.getHID().setRumble(RumbleType.kBothRumble, 1.0); - Leds.getInstance().endgameAlert = true; + leds.endgameAlert = true; }), Commands.waitSeconds(time), Commands.runOnce( () -> { driver.getHID().setRumble(RumbleType.kBothRumble, 0.0); operator.getHID().setRumble(RumbleType.kBothRumble, 0.0); - Leds.getInstance().endgameAlert = false; + leds.endgameAlert = false; })); new Trigger( () -> @@ -370,11 +370,10 @@ private void configureAutos() { "Do Nothing", Commands.runOnce( () -> - RobotState.getInstance() - .resetPose( - new Pose2d( - new Translation2d(), - AllianceFlipUtil.apply(Rotation2d.fromDegrees(180.0)))))); + robotState.resetPose( + new Pose2d( + new Translation2d(), + AllianceFlipUtil.apply(Rotation2d.fromDegrees(180.0)))))); autoChooser.addOption("Davis Ethical Auto", autoBuilder.davisEthicalAuto()); autoChooser.addOption("Davis Alternative Auto", autoBuilder.davisAlternativeAuto()); @@ -443,14 +442,13 @@ private void configureButtonBindings() { Commands.none(), Commands.startEnd( () -> - drive.setHeadingGoal( - () -> RobotState.getInstance().getAimingParameters().driveHeading()), + drive.setHeadingGoal(() -> robotState.getAimingParameters().driveHeading()), drive::clearHeadingGoal), shootAlignDisable); Trigger inWing = new Trigger( () -> - AllianceFlipUtil.apply(RobotState.getInstance().getEstimatedPose().getX()) + AllianceFlipUtil.apply(robotState.getEstimatedPose().getX()) < FieldConstants.wingX); driver .a() @@ -548,7 +546,7 @@ private void configureButtonBindings() { var finalPose = ampCenterRotated.transformBy(GeomUtil.toTransform2d(Units.inchesToMeters(20.0), 0)); double distance = - RobotState.getInstance() + robotState .getEstimatedPose() .getTranslation() .getDistance(finalPose.getTranslation()); @@ -585,9 +583,7 @@ private void configureButtonBindings() { () -> { if (autoDriveDisable.getAsBoolean()) return true; Pose2d poseError = - RobotState.getInstance() - .getEstimatedPose() - .relativeTo(ampAlignedPose.get()); + robotState.getEstimatedPose().relativeTo(ampAlignedPose.get()); return poseError.getTranslation().getNorm() <= Units.feetToMeters(5.0) && Math.abs(poseError.getRotation().getDegrees()) <= 120; }) @@ -621,14 +617,14 @@ private void configureButtonBindings() { operator .povUp() .whileTrue( - Commands.runOnce(() -> RobotState.getInstance().adjustShotCompensationDegrees(0.1)) + Commands.runOnce(() -> robotState.adjustShotCompensationDegrees(0.1)) .andThen(Commands.waitSeconds(0.05)) .ignoringDisable(true) .repeatedly()); operator .povDown() .whileTrue( - Commands.runOnce(() -> RobotState.getInstance().adjustShotCompensationDegrees(-0.1)) + Commands.runOnce(() -> robotState.adjustShotCompensationDegrees(-0.1)) .andThen(Commands.waitSeconds(0.05)) .ignoringDisable(true) .repeatedly()); @@ -667,10 +663,7 @@ private void configureButtonBindings() { // Request amp operator .b() - .whileTrue( - Commands.startEnd( - () -> Leds.getInstance().requestAmp = true, - () -> Leds.getInstance().requestAmp = false)); + .whileTrue(Commands.startEnd(() -> leds.requestAmp = true, () -> leds.requestAmp = false)); // Shuffle gamepiece operator.a().whileTrue(rollers.shuffle()); @@ -735,7 +728,7 @@ public void checkControllers() { public void updateDashboardOutputs() { SmartDashboard.putString( "Shot Compensation Degrees", - String.format("%.1f", RobotState.getInstance().getShotCompensationDegrees())); + String.format("%.1f", robotState.getShotCompensationDegrees())); SmartDashboard.putBoolean("Podium Preset", podiumShotMode); SmartDashboard.putBoolean("Trap Score Mode", trapScoreMode); } From 164e7d08d66a35c8bbbc615426b4b8dc6060e7c7 Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Sat, 9 Mar 2024 12:38:37 -0500 Subject: [PATCH 25/34] Fix CANivore alert --- .../java/org/littletonrobotics/frc2024/Robot.java | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/Robot.java b/src/main/java/org/littletonrobotics/frc2024/Robot.java index 988098c3..e848068c 100644 --- a/src/main/java/org/littletonrobotics/frc2024/Robot.java +++ b/src/main/java/org/littletonrobotics/frc2024/Robot.java @@ -57,14 +57,14 @@ public class Robot extends LoggedRobot { private boolean autoMessagePrinted; private boolean batteryNameWritten = false; private final Timer disabledTimer = new Timer(); - private final Timer canErrorTimer = new Timer(); private final Timer canInitialErrorTimer = new Timer(); + private final Timer canErrorTimer = new Timer(); private final Timer canivoreErrorTimer = new Timer(); private final Alert canErrorAlert = new Alert("CAN errors detected, robot may not be controllable.", AlertType.ERROR); private final Alert canivoreErrorAlert = - new Alert("CANivore error detected, robot may no tbe controllable.", AlertType.ERROR); + new Alert("CANivore error detected, robot may not be controllable.", AlertType.ERROR); private final Alert lowBatteryAlert = new Alert( "Battery voltage is very low, consider turning off the robot or replacing the battery.", @@ -153,8 +153,8 @@ public void robotInit() { }); // Reset alert timers - canErrorTimer.restart(); canInitialErrorTimer.restart(); + canErrorTimer.restart(); canivoreErrorTimer.restart(); disabledTimer.restart(); @@ -245,13 +245,14 @@ public void robotPeriodic() { Logger.recordOutput("CANivoreStatus/TxFullCount", canivoreStatus.TxFullCount); Logger.recordOutput("CANivoreStatus/ReceiveErrorCount", canivoreStatus.REC); Logger.recordOutput("CANivoreStatus/TransmitErrorCount", canivoreStatus.TEC); - // Alerts - if (!canivoreStatus.Status.isOK()) { + if (!canivoreStatus.Status.isOK() + || canStatus.transmitErrorCount > 0 + || canStatus.receiveErrorCount > 0) { canivoreErrorTimer.restart(); } canivoreErrorAlert.set( - !canivoreStatus.Status.isOK() - || !canivoreErrorTimer.hasElapsed(canivoreErrorTimeThreshold)); + !canivoreErrorTimer.hasElapsed(canivoreErrorTimeThreshold) + && !canInitialErrorTimer.hasElapsed(canErrorTimeThreshold)); } // Low battery alert From 9a21bf328dc3a70be8de4989ae4cb663d183818a Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Sat, 9 Mar 2024 13:23:14 -0500 Subject: [PATCH 26/34] Fix endgame alerts --- .../java/org/littletonrobotics/frc2024/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 707832f7..cbcefccc 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -117,9 +117,9 @@ public class RobotContainer { private final Alert overrideDisconnected = new Alert("Override controller disconnected (port 5).", AlertType.INFO); private final LoggedDashboardNumber endgameAlert1 = - new LoggedDashboardNumber("Endgame alert 1", 30.0); + new LoggedDashboardNumber("Endgame Alert #1", 30.0); private final LoggedDashboardNumber endgameAlert2 = - new LoggedDashboardNumber("Endgame alert 2", 10.0); + new LoggedDashboardNumber("Endgame Alert #2", 15.0); private boolean podiumShotMode = false; private boolean trapScoreMode = true; From 21b0da9a0e80bd4fdc0b2fe81757c9edd2666fc2 Mon Sep 17 00:00:00 2001 From: Cameron Earle Date: Sat, 9 Mar 2024 13:33:38 -0500 Subject: [PATCH 27/34] Simplify VTS build into a single image --- trajectory_native/Earthfile | 96 ++++++--------------------- trajectory_native/build_casadi.sh | 14 ++++ trajectory_native/build_ipopt.sh | 23 +++++++ trajectory_native/build_trajoptlib.sh | 16 +++++ trajectory_native/build_vts.sh | 11 +++ 5 files changed, 84 insertions(+), 76 deletions(-) create mode 100644 trajectory_native/build_casadi.sh create mode 100644 trajectory_native/build_ipopt.sh create mode 100644 trajectory_native/build_trajoptlib.sh create mode 100644 trajectory_native/build_vts.sh diff --git a/trajectory_native/Earthfile b/trajectory_native/Earthfile index 77b9c0b6..cf3c8c7d 100644 --- a/trajectory_native/Earthfile +++ b/trajectory_native/Earthfile @@ -1,95 +1,39 @@ VERSION 0.8 FROM scratch -apk-deps: +vts: 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 - 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 - WORKDIR ThirdParty-Mumps-releases-3.0.5 - RUN ./get.Mumps - RUN CC=clang CXX=clang++ ./configure - RUN CC=clang CXX=clang++ make -j8 - # Install to both system (for subsequent ipopt build) and installroot (to copy out as artifacts) - RUN CC=clang CXX=clang++ make install - RUN CC=clang CXX=clang++ make DESTDIR=/RobotCode2024/trajectory_native/ipopt-mumps-build/installroot install - WORKDIR .. - - RUN wget -c https://github.com/coin-or/Ipopt/archive/refs/tags/releases/3.14.14.tar.gz -O - | tar -xz - RUN mkdir Ipopt-releases-3.14.14/build - WORKDIR Ipopt-releases-3.14.14/build - RUN CC=clang CXX=clang++ ../configure - RUN CC=clang CXX=clang++ make -j8 - RUN CC=clang CXX=clang++ make DESTDIR=/RobotCode2024/trajectory_native/ipopt-mumps-build/installroot install - SAVE ARTIFACT /RobotCode2024/trajectory_native/ipopt-mumps-build/installroot - SAVE IMAGE --push ghcr.io/mechanical-advantage/vts-ipopt:latest -casadi: - FROM +apk-deps - 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 - RUN apk add --no-cache --update clang make cmake - RUN CC=clang CXX=clang++ cmake -DWITH_IPOPT=ON -DWITH_DEEPBIND=OFF -DWITH_BUILD_TINYXML=OFF .. - RUN CC=clang CXX=clang++ make -j8 - RUN CC=clang CXX=clang++ make DESTDIR=$(pwd)/installroot install - SAVE ARTIFACT installroot - SAVE IMAGE --push ghcr.io/mechanical-advantage/vts-casadi:latest + COPY --chmod=001 build_ipopt.sh build_ipopt.sh + RUN ./build_ipopt.sh -trajoptlib: - FROM +apk-deps - COPY +ipopt/installroot/usr/local /usr/local/ - COPY +casadi/installroot/usr/local /usr/local/ - LET TRAJOPT_COMMIT=f6cf3d42359f6f41f311f848a4e7f51c3f88c2ca - RUN wget -c https://github.com/SleipnirGroup/TrajoptLib/archive/$TRAJOPT_COMMIT.tar.gz -O - | tar -xz - RUN mkdir TrajoptLib-$TRAJOPT_COMMIT/build - WORKDIR TrajoptLib-$TRAJOPT_COMMIT/build - # Use our CMakeLists.txt (uses system casadi install) - RUN rm ../CMakeLists.txt - COPY trajoptlib-CMakeLists.txt ../CMakeLists.txt - RUN apk add --no-cache --update clang make cmake git - RUN CC=clang CXX=clang++ cmake -DOPTIMIZER_BACKEND=casadi .. - RUN CC=clang CXX=clang++ make -j8 - RUN CC=clang CXX=clang++ make DESTDIR=$(pwd)/installroot install - SAVE ARTIFACT installroot - SAVE IMAGE --push ghcr.io/mechanical-advantage/vts-trajoptlib:latest + COPY --chmod=001 build_casadi.sh . + RUN ./build_casadi.sh -dev-image: - FROM +apk-deps - COPY +ipopt/installroot/usr/local /usr/local/ - COPY +casadi/installroot/usr/local /usr/local/ - COPY +trajoptlib/installroot/usr/local /usr/local/ - 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 + COPY trajoptlib-CMakeLists.txt . + COPY --chmod=001 build_trajoptlib.sh . + RUN ./build_trajoptlib.sh -vts: - FROM +dev-image + WORKDIR vts COPY src src COPY proto proto COPY CMakeLists.txt CMakeLists.txt - RUN mkdir build - WORKDIR build - RUN CC=clang CXX=clang++ cmake .. - RUN CC=clang CXX=clang++ make -j8 + COPY --chmod=001 build_vts.sh build_vts.sh + RUN ./build_vts.sh + EXPOSE 56328 ENV GRPC_VERBOSITY=info + WORKDIR /RobotCode2024/trajectory_native/vts/build ENTRYPOINT ["./trajectory_native"] SAVE IMAGE --push ghcr.io/mechanical-advantage/vts:latest +dev-image: + FROM +vts + RUN apk add --no-cache --update clang make cmake git gdb + ENV CC="clang" + ENV CXX="clang++" + SAVE IMAGE frc6328/vts-dev + vts-all-platforms: - BUILD --platform=linux/amd64 --platform=linux/arm64 +apk-deps - BUILD --platform=linux/amd64 --platform=linux/arm64 +ipopt - BUILD --platform=linux/amd64 --platform=linux/arm64 +casadi - BUILD --platform=linux/amd64 --platform=linux/arm64 +trajoptlib - BUILD --platform=linux/amd64 --platform=linux/arm64 +dev-image BUILD --platform=linux/amd64 --platform=linux/arm64 +vts \ No newline at end of file diff --git a/trajectory_native/build_casadi.sh b/trajectory_native/build_casadi.sh new file mode 100644 index 00000000..228412d2 --- /dev/null +++ b/trajectory_native/build_casadi.sh @@ -0,0 +1,14 @@ +#!/bin/sh +apk add --update clang make cmake blas-dev lapack-dev tinyxml2-dev + +CC=clang +CXX=clang++ +CASADI_VERSION="3.6.4" + +wget -c https://github.com/casadi/casadi/archive/refs/tags/$CASADI_VERSION.tar.gz -O - | tar -xz +mkdir casadi-$CASADI_VERSION/build +cd casadi-$CASADI_VERSION/build +cmake -DWITH_IPOPT=ON -DWITH_DEEPBIND=OFF -DWITH_BUILD_TINYXML=OFF .. +make -j$(nproc) +make install +cd ../.. \ No newline at end of file diff --git a/trajectory_native/build_ipopt.sh b/trajectory_native/build_ipopt.sh new file mode 100644 index 00000000..1d7ffe3d --- /dev/null +++ b/trajectory_native/build_ipopt.sh @@ -0,0 +1,23 @@ +#!/bin/sh +apk add --update clang make gfortran blas-dev lapack-dev + +CC=clang +CXX=clang++ +MUMPS_VERSION="3.0.5" +IPOPT_VERSION="3.14.14" + +wget -c https://github.com/coin-or-tools/ThirdParty-Mumps/archive/refs/tags/releases/$MUMPS_VERSION.tar.gz -O - | tar -xz +cd ThirdParty-Mumps-releases-3.0.5 +./get.Mumps +./configure +make -j$(nproc) +make install +cd .. + +wget -c https://github.com/coin-or/Ipopt/archive/refs/tags/releases/$IPOPT_VERSION.tar.gz -O - | tar -xz +mkdir Ipopt-releases-$IPOPT_VERSION/build +cd Ipopt-releases-$IPOPT_VERSION/build +../configure +make -j$(nproc) +make install +cd ../.. \ No newline at end of file diff --git a/trajectory_native/build_trajoptlib.sh b/trajectory_native/build_trajoptlib.sh new file mode 100644 index 00000000..21f3b3f6 --- /dev/null +++ b/trajectory_native/build_trajoptlib.sh @@ -0,0 +1,16 @@ +#!/bin/sh +apk add --update clang make cmake git + +CC=clang +CXX=clang++ +TRAJOPT_COMMIT=f6cf3d42359f6f41f311f848a4e7f51c3f88c2ca + +wget -c https://github.com/SleipnirGroup/TrajoptLib/archive/$TRAJOPT_COMMIT.tar.gz -O - | tar -xz +mkdir TrajoptLib-$TRAJOPT_COMMIT/build +cd TrajoptLib-$TRAJOPT_COMMIT/build +rm ../CMakeLists.txt +cp ../../trajoptlib-CMakeLists.txt ../CMakeLists.txt +cmake -DOPTIMIZER_BACKEND=casadi -DBUILD_TESTING=OFF .. +make -j$(nproc) +make install +cd ../.. \ No newline at end of file diff --git a/trajectory_native/build_vts.sh b/trajectory_native/build_vts.sh new file mode 100644 index 00000000..aeda3232 --- /dev/null +++ b/trajectory_native/build_vts.sh @@ -0,0 +1,11 @@ +#!/bin/sh +apk add --update clang make cmake grpc-dev protobuf-dev nlohmann-json fmt-dev + +CC=clang +CXX=clang++ + +mkdir build +cd build +cmake .. +make -j$(nproc) +cd .. \ No newline at end of file From 1f6d6015dff76dfc2b86cf1839a731496f0bd58b Mon Sep 17 00:00:00 2001 From: Cameron Earle Date: Sat, 9 Mar 2024 13:44:10 -0500 Subject: [PATCH 28/34] Add trailing newline to trajoptlib-CMakeLists.txt --- trajectory_native/trajoptlib-CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/trajectory_native/trajoptlib-CMakeLists.txt b/trajectory_native/trajoptlib-CMakeLists.txt index 6170131e..ab938160 100644 --- a/trajectory_native/trajoptlib-CMakeLists.txt +++ b/trajectory_native/trajoptlib-CMakeLists.txt @@ -268,4 +268,4 @@ foreach(example ${EXAMPLES}) catch_discover_tests(${example}Test) endif() endif() -endforeach() \ No newline at end of file +endforeach() From 96a83c02798e31eada70d6a70c38dca7a04ea44f Mon Sep 17 00:00:00 2001 From: Cameron Earle Date: Sat, 9 Mar 2024 14:09:57 -0500 Subject: [PATCH 29/34] Set permissions on all VTS image copied files --- trajectory_native/Earthfile | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/trajectory_native/Earthfile b/trajectory_native/Earthfile index cf3c8c7d..1a409bf6 100644 --- a/trajectory_native/Earthfile +++ b/trajectory_native/Earthfile @@ -11,14 +11,14 @@ vts: COPY --chmod=001 build_casadi.sh . RUN ./build_casadi.sh - COPY trajoptlib-CMakeLists.txt . + COPY --chmod=755 trajoptlib-CMakeLists.txt . COPY --chmod=001 build_trajoptlib.sh . RUN ./build_trajoptlib.sh WORKDIR vts - COPY src src - COPY proto proto - COPY CMakeLists.txt CMakeLists.txt + COPY --chmod=755 src src + COPY --chmod=755 proto proto + COPY --chmod=755 CMakeLists.txt CMakeLists.txt COPY --chmod=001 build_vts.sh build_vts.sh RUN ./build_vts.sh From 2cfc8bb0befecf535a9da267179e1054b021b687 Mon Sep 17 00:00:00 2001 From: Cameron Earle Date: Sat, 9 Mar 2024 14:53:33 -0500 Subject: [PATCH 30/34] Fix VTS Ipopt build --- trajectory_native/build_ipopt.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/trajectory_native/build_ipopt.sh b/trajectory_native/build_ipopt.sh index 1d7ffe3d..ce1f79b4 100644 --- a/trajectory_native/build_ipopt.sh +++ b/trajectory_native/build_ipopt.sh @@ -1,5 +1,5 @@ #!/bin/sh -apk add --update clang make gfortran blas-dev lapack-dev +apk add --update clang make patch gfortran blas-dev lapack-dev CC=clang CXX=clang++ @@ -7,7 +7,7 @@ MUMPS_VERSION="3.0.5" IPOPT_VERSION="3.14.14" wget -c https://github.com/coin-or-tools/ThirdParty-Mumps/archive/refs/tags/releases/$MUMPS_VERSION.tar.gz -O - | tar -xz -cd ThirdParty-Mumps-releases-3.0.5 +cd ThirdParty-Mumps-releases-$MUMPS_VERSION ./get.Mumps ./configure make -j$(nproc) From dd353494d7a1b2c9710329fa0a4a591db00040d6 Mon Sep 17 00:00:00 2001 From: Cameron Date: Sat, 9 Mar 2024 16:05:57 -0500 Subject: [PATCH 31/34] Critical pose fudge factors (#182) * Add critical pose fudge factors * Major spotless malfunction --- .../frc2024/FudgeFactors.java | 40 +++++++++++++++++++ .../frc2024/RobotContainer.java | 4 +- .../littletonrobotics/frc2024/RobotState.java | 3 +- .../frc2024/commands/ClimbingCommands.java | 16 ++++++-- 4 files changed, 58 insertions(+), 5 deletions(-) create mode 100644 src/main/java/org/littletonrobotics/frc2024/FudgeFactors.java diff --git a/src/main/java/org/littletonrobotics/frc2024/FudgeFactors.java b/src/main/java/org/littletonrobotics/frc2024/FudgeFactors.java new file mode 100644 index 00000000..d640bc46 --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/FudgeFactors.java @@ -0,0 +1,40 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package org.littletonrobotics.frc2024; + +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.wpilibj.DriverStation; +import lombok.Builder; + +/** Fudge factors for critical field locations which can be tuned per alliance at events. */ +public final class FudgeFactors { + @Builder + public static class FudgedTransform { + @Builder.Default private final Transform2d red = new Transform2d(); + + @Builder.Default private final Transform2d blue = new Transform2d(); + + public Transform2d getTransform() { + return DriverStation.getAlliance() + .map(alliance -> alliance == DriverStation.Alliance.Red ? red : blue) + .orElseGet(Transform2d::new); + } + } + + private FudgeFactors() {} + + public static final FudgedTransform speaker = FudgedTransform.builder().build(); + + public static final FudgedTransform amp = FudgedTransform.builder().build(); + + public static final FudgedTransform centerPodiumAmpChain = FudgedTransform.builder().build(); + + public static final FudgedTransform centerAmpSourceChain = FudgedTransform.builder().build(); + + public static final FudgedTransform centerSourcePodiumChain = FudgedTransform.builder().build(); +} diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index cbcefccc..39078c7b 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -544,7 +544,9 @@ private void configureButtonBindings() { AllianceFlipUtil.apply( new Pose2d(FieldConstants.ampCenter, new Rotation2d(-Math.PI / 2.0))); var finalPose = - ampCenterRotated.transformBy(GeomUtil.toTransform2d(Units.inchesToMeters(20.0), 0)); + ampCenterRotated + .transformBy(GeomUtil.toTransform2d(Units.inchesToMeters(20.0), 0)) + .transformBy(FudgeFactors.amp.getTransform()); double distance = robotState .getEstimatedPose() diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java index 4d32e716..d485ff24 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotState.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotState.java @@ -212,7 +212,8 @@ public AimingParameters getAimingParameters() { Transform2d fieldToTarget = AllianceFlipUtil.apply(FieldConstants.Speaker.centerSpeakerOpening) .toTranslation2d() - .toTransform2d(); + .toTransform2d() + .plus(FudgeFactors.speaker.getTransform()); Pose2d fieldToPredictedVehicle = lookaheadDisable.getAsBoolean() ? getEstimatedPose() diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java b/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java index ee25504f..929a8532 100644 --- a/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java +++ b/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java @@ -20,6 +20,7 @@ import java.util.function.DoubleSupplier; import java.util.function.Supplier; import lombok.experimental.ExtensionMethod; +import org.littletonrobotics.frc2024.FudgeFactors; import org.littletonrobotics.frc2024.RobotState; import org.littletonrobotics.frc2024.subsystems.drive.Drive; import org.littletonrobotics.frc2024.subsystems.rollers.Rollers; @@ -38,16 +39,25 @@ public class ClimbingCommands { private static final LoggedTunableNumber chainToFront = new LoggedTunableNumber("ClimbingCommands/ChainToFrontOffset", 0.9); - private static final List centeredClimbedPosesNoOffset = + private static final List> centeredClimbedPosesNoOffset = List.of( - Stage.centerPodiumAmpChain, Stage.centerAmpSourceChain, Stage.centerSourcePodiumChain); + () -> + Stage.centerPodiumAmpChain.transformBy( + FudgeFactors.centerPodiumAmpChain.getTransform()), + () -> + Stage.centerAmpSourceChain.transformBy( + FudgeFactors.centerAmpSourceChain.getTransform()), + () -> + Stage.centerSourcePodiumChain.transformBy( + FudgeFactors.centerSourcePodiumChain.getTransform())); private static final Supplier nearestClimbedPose = () -> { Pose2d currentPose = RobotState.getInstance().getEstimatedPose(); List climbedPoses = centeredClimbedPosesNoOffset.stream() - .map(pose -> AllianceFlipUtil.apply(pose)) + .map(Supplier::get) + .map(AllianceFlipUtil::apply) .toList(); return currentPose.nearest(climbedPoses); }; From 38f86538121b2a6ecdb3af1abe332c99cd5a326a Mon Sep 17 00:00:00 2001 From: Surya Thoppae <63197592+suryatho@users.noreply.github.com> Date: Sat, 9 Mar 2024 16:07:21 -0500 Subject: [PATCH 32/34] Add seperate compensation for auto shots (#178) --- .../frc2024/commands/auto/AutoBuilder.java | 23 ++++++++----------- .../superstructure/Superstructure.java | 7 ++++++ .../subsystems/superstructure/arm/Arm.java | 4 +++- 3 files changed, 20 insertions(+), 14 deletions(-) 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 0bfe91e5..4d9fb298 100644 --- a/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java +++ b/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoBuilder.java @@ -71,13 +71,13 @@ public Command davisEthicalAuto() { rollers .setGoalCommand(Rollers.Goal.FEED_TO_SHOOTER) .until(() -> autoTimer.hasElapsed(preloadDelay))) - .deadlineWith(superstructure.setGoalCommand(Superstructure.Goal.AIM)), + .deadlineWith(superstructure.aimWithCompensation(0.0)), // Intake centerline 0 waitUntilXCrossed(FieldConstants.wingX + 0.05, true) .andThen(waitUntilXCrossed(FieldConstants.wingX, false)) .deadlineWith( - superstructure.setGoalCommand(Superstructure.Goal.INTAKE), + superstructure.aimWithCompensation(0.0), rollers.setGoalCommand(Rollers.Goal.FLOOR_INTAKE)), // Shoot centerline 0 @@ -91,7 +91,7 @@ public Command davisEthicalAuto() { rollers .setGoalCommand(Rollers.Goal.FEED_TO_SHOOTER) .withTimeout(shootTimeoutSecs.get())) - .deadlineWith(superstructure.setGoalCommand(Superstructure.Goal.AIM)), + .deadlineWith(superstructure.aimWithCompensation(0.0)), // Intake centerline 1 waitUntilXCrossed( @@ -123,8 +123,7 @@ public Command davisEthicalAuto() { .withTimeout(shootTimeoutSecs.get())) .deadlineWith( waitUntilXCrossed(FieldConstants.Stage.center.getX(), false) - .andThen( - superstructure.setGoalCommand(Superstructure.Goal.AIM))), + .andThen(superstructure.aimWithCompensation(0.0))), // Intake centerline 2 waitUntilXCrossed( @@ -157,8 +156,7 @@ public Command davisEthicalAuto() { .withTimeout(shootTimeoutSecs.get())) .deadlineWith( waitUntilXCrossed(FieldConstants.Stage.center.getX(), false) - .andThen( - superstructure.setGoalCommand(Superstructure.Goal.AIM))))) + .andThen(superstructure.aimWithCompensation(0.0))))) // Run flywheels .deadlineWith(flywheels.shootCommand())); @@ -204,7 +202,7 @@ public Command davisAlternativeAuto() { rollers .setGoalCommand(Rollers.Goal.FEED_TO_SHOOTER) .until(() -> autoTimer.hasElapsed(preloadDelay))) - .deadlineWith(superstructure.setGoalCommand(Superstructure.Goal.AIM)), + .deadlineWith(superstructure.aimWithCompensation(0.0)), // Intake spike Commands.parallel( @@ -217,7 +215,7 @@ public Command davisAlternativeAuto() { // Shoot spike superstructure - .setGoalCommand(Superstructure.Goal.AIM) + .aimWithCompensation(0.0) .alongWith( Commands.waitSeconds(spikeAimDelay) .andThen(rollers.setGoalCommand(Rollers.Goal.FEED_TO_SHOOTER))) @@ -252,7 +250,7 @@ public Command davisAlternativeAuto() { rollers .setGoalCommand(Rollers.Goal.FEED_TO_SHOOTER) .withTimeout(shootTimeoutSecs.get())) - .deadlineWith(superstructure.setGoalCommand(Superstructure.Goal.AIM)), + .deadlineWith(superstructure.aimWithCompensation(0.0)), // Intake centerline 3 waitUntilXCrossed(FieldConstants.wingX + 0.85, true) @@ -277,7 +275,7 @@ public Command davisAlternativeAuto() { rollers .setGoalCommand(Rollers.Goal.FEED_TO_SHOOTER) .withTimeout(shootTimeoutSecs.get())) - .deadlineWith(superstructure.setGoalCommand(Superstructure.Goal.AIM)), + .deadlineWith(superstructure.aimWithCompensation(0.0)), // Intake centerline 2 waitUntilXCrossed( @@ -314,8 +312,7 @@ public Command davisAlternativeAuto() { .withTimeout(shootTimeoutSecs.get())) .deadlineWith( waitUntilXCrossed(FieldConstants.Stage.center.getX() + 0.1, false) - .andThen( - superstructure.setGoalCommand(Superstructure.Goal.AIM))))) + .andThen(superstructure.aimWithCompensation(0.0))))) // Run flywheels .deadlineWith(flywheels.shootCommand())); 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 b65b57b2..be8b83ab 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java @@ -206,6 +206,13 @@ public Command setGoalWithConstraintsCommand( .finallyDo(() -> arm.setProfileConstraints(Arm.maxProfileConstraints.get())); } + /** Command to aim the superstructure with a compensation value in degrees */ + public Command aimWithCompensation(double compensation) { + return setGoalCommand(Goal.AIM) + .beforeStarting(() -> arm.setCurrentCompensation(compensation)) + .finallyDo(() -> arm.setCurrentCompensation(0.0)); + } + @AutoLogOutput(key = "Superstructure/CompletedGoal") public boolean atGoal() { return currentGoal == desiredGoal && arm.atGoal() && climber.atGoal(); 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 de7d6c98..a2ad9050 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 @@ -101,6 +101,7 @@ private double getRads() { private final ArmIO io; private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); + @AutoLogOutput @Setter private double currentCompensation = 0.0; private TrapezoidProfile.Constraints currentConstraints = maxProfileConstraints.get(); private TrapezoidProfile profile; private TrapezoidProfile.State setpointState = new TrapezoidProfile.State(); @@ -188,7 +189,8 @@ public void periodic() { setpointState, new TrapezoidProfile.State( MathUtil.clamp( - goal.getRads(), + goal.getRads() + + (goal == Goal.AIM ? Units.degreesToRadians(currentCompensation) : 0.0), Units.degreesToRadians(lowerLimitDegrees.get()), Units.degreesToRadians(upperLimitDegrees.get())), 0.0)); From a8896551ac43388747e620aca42181b4ac64b1d3 Mon Sep 17 00:00:00 2001 From: suryatho Date: Fri, 8 Mar 2024 00:04:43 -0500 Subject: [PATCH 33/34] Add drive static characterization --- .../org/littletonrobotics/frc2024/RobotContainer.java | 5 +++++ .../littletonrobotics/frc2024/subsystems/drive/Drive.java | 4 ++-- .../frc2024/subsystems/drive/Module.java | 4 ++-- .../frc2024/subsystems/drive/ModuleIO.java | 7 ++----- .../frc2024/subsystems/drive/ModuleIOKrakenFOC.java | 8 ++++++-- .../frc2024/subsystems/drive/ModuleIOSim.java | 5 +++++ .../frc2024/subsystems/drive/ModuleIOSparkMax.java | 5 +++++ 7 files changed, 27 insertions(+), 11 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 39078c7b..15872d46 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -378,6 +378,11 @@ private void configureAutos() { autoChooser.addOption("Davis Alternative Auto", autoBuilder.davisAlternativeAuto()); // Set up feedforward characterization + autoChooser.addOption( + "Drive Static Characterization", + new StaticCharacterization( + drive, drive::runCharacterization, drive::getCharacterizationVelocity) + .finallyDo(drive::endCharacterization)); autoChooser.addOption( "Drive FF Characterization", new FeedForwardCharacterization( diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java index 8e9efe65..69ea433f 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java @@ -368,9 +368,9 @@ public boolean atHeadingGoal() { } /** Runs forwards at the commanded voltage or amps. */ - public void runCharacterization(double volts) { + public void runCharacterization(double input) { currentDriveMode = DriveMode.CHARACTERIZATION; - characterizationInput = volts; + characterizationInput = input; } /** Disables the characterization mode. */ diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java index b9832a30..c30a27fc 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java @@ -89,9 +89,9 @@ public void runSetpoint(SwerveModuleState setpoint, SwerveModuleState torqueFF) } /** Runs characterization volts or amps depending on using voltage or current control. */ - public void runCharacterization(double turnSetpointRads, double volts) { + public void runCharacterization(double turnSetpointRads, double input) { io.runTurnPositionSetpoint(turnSetpointRads); - io.runDriveVolts(volts); + io.runCharacterization(input); } /** Stops motors. */ diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIO.java index ad9e184e..b7096737 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIO.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIO.java @@ -43,11 +43,8 @@ default void runDriveVolts(double volts) {} /** Run turn motor at volts */ default void runTurnVolts(double volts) {} - /** Run drive motor at current */ - default void runDriveCurrent(double current) {} - - /** Run turn motor at current */ - default void runTurnCurrent(double current) {} + /** Run characterization input (amps or volts) into drive motor */ + default void runCharacterization(double input) {} /** Run to drive velocity setpoint with feedforward */ default void runDriveVelocitySetpoint(double velocityRadsPerSec, double feedForward) {} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java index 880e8faa..96d9467d 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java @@ -55,8 +55,7 @@ public class ModuleIOKrakenFOC implements ModuleIO { // Control private final VoltageOut voltageControl = new VoltageOut(0).withUpdateFreqHz(0); - private final VelocityVoltage velocityControl = - new VelocityVoltage(0).withEnableFOC(true).withUpdateFreqHz(0); + private final TorqueCurrentFOC currentControl = new TorqueCurrentFOC(0).withUpdateFreqHz(0); private final VelocityTorqueCurrentFOC velocityTorqueCurrentFOC = new VelocityTorqueCurrentFOC(0).withUpdateFreqHz(0); private final PositionTorqueCurrentFOC positionControl = @@ -192,6 +191,11 @@ public void runTurnVolts(double volts) { turnTalon.setControl(voltageControl.withOutput(volts)); } + @Override + public void runCharacterization(double input) { + driveTalon.setControl(currentControl.withOutput(input)); + } + @Override public void runDriveVelocitySetpoint(double velocityRadsPerSec, double feedForward) { driveTalon.setControl( diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java index 1ae65d47..66e307af 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java @@ -74,6 +74,11 @@ public void runTurnVolts(double volts) { turnSim.setInputVoltage(turnAppliedVolts); } + @Override + public void runCharacterization(double input) { + runDriveVolts(input); + } + @Override public void runDriveVelocitySetpoint(double velocityRadsPerSec, double feedForward) { runDriveVolts( diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSparkMax.java index 5a0c9ea2..cb62ba26 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSparkMax.java @@ -140,6 +140,11 @@ public void runTurnVolts(double volts) { turnMotor.setVoltage(volts); } + @Override + public void runCharacterization(double input) { + runDriveVolts(input); + } + @Override public void runDriveVelocitySetpoint(double velocityRadsPerSec, double feedForward) { double feedback = From f316d57ea2ebb2fa95f6ff15a386de0e476ae7ff Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Sat, 9 Mar 2024 17:17:51 -0500 Subject: [PATCH 34/34] Trap repeatedly (#185) --- .../frc2024/commands/ClimbingCommands.java | 43 +++++++++++-------- 1 file changed, 25 insertions(+), 18 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java b/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java index 929a8532..6749033b 100644 --- a/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java +++ b/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java @@ -208,24 +208,31 @@ public static Command climbNTrapSequence( Commands.waitUntil(trapScoreTrigger) .andThen(Commands.waitUntil(trapScoreTrigger.negate()))), - // Extend backpack - superstructure - .setGoalCommand(Superstructure.Goal.TRAP) - .alongWith(rollers.setGoalCommand(Rollers.Goal.TRAP_PRESCORE)) - .raceWith( - Commands.waitUntil(trapScoreTrigger) - .andThen(Commands.waitUntil(trapScoreTrigger.negate()))), - - // Score in trap and wait - rollers - .setGoalCommand(Rollers.Goal.TRAP_SCORE) - .alongWith(superstructure.setGoalCommand(Superstructure.Goal.TRAP)) - .raceWith( - Commands.waitUntil(trapScoreTrigger) - .andThen(Commands.waitUntil(trapScoreTrigger.negate()))), - - // Retract backpack - superstructure.setGoalCommand(Superstructure.Goal.CLIMB)) + // Repeat trap sequence forever and ever + Commands.sequence( + // Extend backpack + superstructure + .setGoalCommand(Superstructure.Goal.TRAP) + .alongWith(rollers.setGoalCommand(Rollers.Goal.TRAP_PRESCORE)) + .raceWith( + Commands.waitUntil(trapScoreTrigger) + .andThen(Commands.waitUntil(trapScoreTrigger.negate()))), + + // Score in trap and wait + rollers + .setGoalCommand(Rollers.Goal.TRAP_SCORE) + .alongWith(superstructure.setGoalCommand(Superstructure.Goal.TRAP)) + .raceWith( + Commands.waitUntil(trapScoreTrigger) + .andThen(Commands.waitUntil(trapScoreTrigger.negate()))), + + // Retract backpack + superstructure + .setGoalCommand(Superstructure.Goal.CLIMB) + .raceWith( + Commands.waitUntil(trapScoreTrigger) + .andThen(Commands.waitUntil(trapScoreTrigger.negate())))) + .repeatedly()) // If cancelled, go to safe state .finallyDo(