Merge remote-tracking branch 'origin/main' into spike_autos

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;
@@ -59,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.",
@@ -126,7 +124,6 @@ public void robotInit() {
 
     // Start AdvantageKit logger
     Logger.start();
-    Leds.getInstance();
 
     // Log active commands
     Map<String, Integer> commandCounts = new HashMap<>();
@@ -155,13 +152,9 @@ public void robotInit() {
               logCommandFunction.accept(command, false);
             });
 
-    // Default to blue alliance in sim
-    if (Constants.getMode() == Constants.Mode.SIM) {
-      DriverStationSim.setAllianceStationId(AllianceStationID.Blue1);
-    }
-
-    canErrorTimer.restart();
+    // Reset alert timers
     canInitialErrorTimer.restart();
+    canErrorTimer.restart();
     canivoreErrorTimer.restart();
     disabledTimer.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();
   }
 
@@ -255,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

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();
 
@@ -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;
@@ -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());
     autoChooser.addOption("Source FRC6328 Auto", autoBuilder.sourceFRC6328Auto());
@@ -446,14 +445,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()
@@ -551,7 +549,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());
@@ -588,9 +586,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;
                             })
@@ -624,14 +620,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());
@@ -670,10 +666,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());
@@ -738,7 +731,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);
   }

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
+    COPY --chmod=001 build_ipopt.sh build_ipopt.sh
+    RUN ./build_ipopt.sh
 
-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_casadi.sh .
+    RUN ./build_casadi.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=755 trajoptlib-CMakeLists.txt .
+    COPY --chmod=001 build_trajoptlib.sh .
+    RUN ./build_trajoptlib.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
+    WORKDIR vts
+    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
 
-vts:
-    FROM +dev-image
-    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
     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

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 ../..

trajectory_native/build_ipopt.sh

@@ -0,0 +1,23 @@
+#!/bin/sh
+apk add --update clang make patch 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-$MUMPS_VERSION
+./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 ../..