diff --git a/src/main/java/org/littletonrobotics/frc2024/Robot.java b/src/main/java/org/littletonrobotics/frc2024/Robot.java index ba09d3e6..e848068c 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; @@ -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 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 diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 4b3e88e1..46c028c6 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(); @@ -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); } diff --git a/trajectory_native/Earthfile b/trajectory_native/Earthfile index 77b9c0b6..1a409bf6 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 + 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 \ 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..ce1f79b4 --- /dev/null +++ b/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 ../.. \ 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 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()