diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml index 9e9769e527d..e0e56438f49 100644 --- a/.github/workflows/cmake.yml +++ b/.github/workflows/cmake.yml @@ -88,28 +88,13 @@ jobs: uses: lukka/run-vcpkg@v11.1 with: vcpkgDirectory: ${{ runner.workspace }}/vcpkg - vcpkgGitCommitId: 78b61582c9e093fda56a01ebb654be15a0033897 # HEAD on 2023-08-6 + vcpkgGitCommitId: 37c3e63a1306562f7f59c4c3c8892ddd50fdf992 # HEAD on 2024-02-24 - name: configure - run: cmake -S . -B build -G "Ninja" -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_BUILD_TYPE=Release -DWITH_JAVA=OFF -DWITH_EXAMPLES=ON -DUSE_SYSTEM_FMTLIB=ON -DUSE_SYSTEM_LIBUV=ON -DUSE_SYSTEM_EIGEN=ON -DCMAKE_TOOLCHAIN_FILE=${{ runner.workspace }}/vcpkg/scripts/buildsystems/vcpkg.cmake -DVCPKG_INSTALL_OPTIONS=--clean-after-build -DVCPKG_TARGET_TRIPLET=x64-windows-release -DVCPKG_HOST_TRIPLET=x64-windows-release + run: cmake -S . -B build -G "Ninja" -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_BUILD_TYPE=Release -DWITH_JAVA=OFF -DWITH_EXAMPLES=ON -DUSE_SYSTEM_FMTLIB=ON -DUSE_SYSTEM_LIBUV=ON -DUSE_SYSTEM_EIGEN=OFF -DCMAKE_TOOLCHAIN_FILE=${{ runner.workspace }}/vcpkg/scripts/buildsystems/vcpkg.cmake -DVCPKG_INSTALL_OPTIONS=--clean-after-build -DVCPKG_TARGET_TRIPLET=x64-windows-release -DVCPKG_HOST_TRIPLET=x64-windows-release env: SCCACHE_GHA_ENABLED: "true" - # Build wpiutil at full speed, wpimath depends on wpiutil - - name: build wpiutil - working-directory: build - run: cmake --build . --parallel $(nproc) --target wpiutil/all - env: - SCCACHE_GHA_ENABLED: "true" - - # Build wpimath slow to prevent OOM - - name: build wpimath - working-directory: build - run: cmake --build . --parallel 1 --target wpimath/all - env: - SCCACHE_GHA_ENABLED: "true" - - # Build everything else fast - name: build working-directory: build run: cmake --build . --parallel $(nproc) diff --git a/.github/workflows/lint-format.yml b/.github/workflows/lint-format.yml index afbcde895c1..abff71e07fc 100644 --- a/.github/workflows/lint-format.yml +++ b/.github/workflows/lint-format.yml @@ -27,7 +27,7 @@ jobs: with: python-version: '3.10' - name: Install wpiformat - run: pip3 install wpiformat==2023.36 + run: pip3 install wpiformat==2024.32 - name: Run run: wpiformat - name: Check output diff --git a/apriltag/convert_apriltag_layouts.py b/apriltag/convert_apriltag_layouts.py index 2c1d60e411d..b89f666cf35 100755 --- a/apriltag/convert_apriltag_layouts.py +++ b/apriltag/convert_apriltag_layouts.py @@ -82,6 +82,7 @@ def main(): # Write JSON with open(filename.replace(".csv", ".json"), "w") as f: json.dump(json_data, f, indent=2) + f.write("\n") if __name__ == "__main__": diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java index 5d0e4248878..d2a5dd1cdfe 100644 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java +++ b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java @@ -16,6 +16,7 @@ import java.io.IOException; import java.io.InputStream; import java.io.InputStreamReader; +import java.io.UncheckedIOException; import java.nio.file.Path; import java.util.ArrayList; import java.util.HashMap; @@ -221,6 +222,22 @@ public void serialize(Path path) throws IOException { new ObjectMapper().writeValue(path.toFile(), this); } + /** + * Get an official {@link AprilTagFieldLayout}. + * + * @param field The loadable AprilTag field layout. + * @return AprilTagFieldLayout of the field. + * @throws UncheckedIOException If the layout does not exist. + */ + public static AprilTagFieldLayout loadField(AprilTagFields field) { + try { + return loadFromResource(field.m_resourceFile); + } catch (IOException e) { + throw new UncheckedIOException( + "Could not load AprilTagFieldLayout from " + field.m_resourceFile, e); + } + } + /** * Deserializes a field layout from a resource within a internal jar file. * diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java index 653151e6266..776050c2749 100644 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java +++ b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java @@ -4,7 +4,6 @@ package edu.wpi.first.apriltag; -import java.io.IOException; import java.io.UncheckedIOException; /** Loadable AprilTag field layouts. */ @@ -36,11 +35,6 @@ public enum AprilTagFields { * @throws UncheckedIOException If the layout does not exist */ public AprilTagFieldLayout loadAprilTagLayoutField() { - try { - return AprilTagFieldLayout.loadFromResource(m_resourceFile); - } catch (IOException e) { - throw new UncheckedIOException( - "Could not load AprilTagFieldLayout from " + m_resourceFile, e); - } + return AprilTagFieldLayout.loadField(this); } } diff --git a/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp b/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp index 59b30ec30eb..c0b4ca233bb 100644 --- a/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp +++ b/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp @@ -125,3 +125,37 @@ void frc::from_json(const wpi::json& json, AprilTagFieldLayout& layout) { layout.m_fieldWidth = units::meter_t{json.at("field").at("width").get()}; } + +// Use namespace declaration for forward declaration +namespace frc { + +// C++ generated from resource files +std::string_view GetResource_2022_rapidreact_json(); +std::string_view GetResource_2023_chargedup_json(); +std::string_view GetResource_2024_crescendo_json(); + +} // namespace frc + +AprilTagFieldLayout AprilTagFieldLayout::LoadField(AprilTagField field) { + std::string_view fieldString; + switch (field) { + case AprilTagField::k2022RapidReact: + fieldString = GetResource_2022_rapidreact_json(); + break; + case AprilTagField::k2023ChargedUp: + fieldString = GetResource_2023_chargedup_json(); + break; + case AprilTagField::k2024Crescendo: + fieldString = GetResource_2024_crescendo_json(); + break; + case AprilTagField::kNumFields: + throw std::invalid_argument("Invalid Field"); + } + + wpi::json json = wpi::json::parse(fieldString); + return json.get(); +} + +AprilTagFieldLayout frc::LoadAprilTagLayoutField(AprilTagField field) { + return AprilTagFieldLayout::LoadField(field); +} diff --git a/apriltag/src/main/native/cpp/AprilTagFields.cpp b/apriltag/src/main/native/cpp/AprilTagFields.cpp deleted file mode 100644 index be512caa00b..00000000000 --- a/apriltag/src/main/native/cpp/AprilTagFields.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "frc/apriltag/AprilTagFields.h" - -#include - -namespace frc { - -// C++ generated from resource files -std::string_view GetResource_2022_rapidreact_json(); -std::string_view GetResource_2023_chargedup_json(); -std::string_view GetResource_2024_crescendo_json(); - -AprilTagFieldLayout LoadAprilTagLayoutField(AprilTagField field) { - std::string_view fieldString; - switch (field) { - case AprilTagField::k2022RapidReact: - fieldString = GetResource_2022_rapidreact_json(); - break; - case AprilTagField::k2023ChargedUp: - fieldString = GetResource_2023_chargedup_json(); - break; - case AprilTagField::k2024Crescendo: - fieldString = GetResource_2024_crescendo_json(); - break; - case AprilTagField::kNumFields: - throw std::invalid_argument("Invalid Field"); - } - - wpi::json json = wpi::json::parse(fieldString); - return json.get(); -} - -} // namespace frc diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index 1f5397b25a2..6ba3824ed66 100644 --- a/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -14,6 +14,7 @@ #include #include "frc/apriltag/AprilTag.h" +#include "frc/apriltag/AprilTagFields.h" #include "frc/geometry/Pose3d.h" namespace frc { @@ -48,6 +49,14 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { kRedAllianceWallRightSide, }; + /** + * Loads an AprilTagFieldLayout from a predefined field + * + * @param field The predefined field + * @return AprilTagFieldLayout of the field + */ + static AprilTagFieldLayout LoadField(AprilTagField field); + AprilTagFieldLayout() = default; /** @@ -152,4 +161,13 @@ void to_json(wpi::json& json, const AprilTagFieldLayout& layout); WPILIB_DLLEXPORT void from_json(const wpi::json& json, AprilTagFieldLayout& layout); +/** + * Loads an AprilTagFieldLayout from a predefined field + * + * @param field The predefined field + * @return AprilTagFieldLayout of the field + */ +WPILIB_DLLEXPORT AprilTagFieldLayout +LoadAprilTagLayoutField(AprilTagField field); + } // namespace frc diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h index 933cf287ae3..6ef0930e9b6 100644 --- a/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h +++ b/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h @@ -8,8 +8,6 @@ #include -#include "frc/apriltag/AprilTagFieldLayout.h" - namespace frc { /** @@ -28,12 +26,4 @@ enum class AprilTagField { kNumFields, }; -/** - * Loads an AprilTagFieldLayout from a predefined field - * - * @param field The predefined field - */ -WPILIB_DLLEXPORT AprilTagFieldLayout -LoadAprilTagLayoutField(AprilTagField field); - } // namespace frc diff --git a/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.json b/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.json index 0da1bac4049..8cb837a5023 100644 --- a/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.json +++ b/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.json @@ -290,7 +290,7 @@ } ], "field": { - "length": 16.451, + "length": 16.541, "width": 8.211 } } diff --git a/apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java b/apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java index d5deb573af9..e4e4875a194 100644 --- a/apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java +++ b/apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java @@ -22,13 +22,14 @@ class LoadConfigTest { @ParameterizedTest @EnumSource(AprilTagFields.class) void testLoad(AprilTagFields field) { - AprilTagFieldLayout layout = Assertions.assertDoesNotThrow(field::loadAprilTagLayoutField); + AprilTagFieldLayout layout = + Assertions.assertDoesNotThrow(() -> AprilTagFieldLayout.loadField(field)); assertNotNull(layout); } @Test void test2022RapidReact() { - AprilTagFieldLayout layout = AprilTagFields.k2022RapidReact.loadAprilTagLayoutField(); + AprilTagFieldLayout layout = AprilTagFieldLayout.loadField(AprilTagFields.k2022RapidReact); // Blue Hangar Truss - Hub Pose3d expectedPose = diff --git a/apriltag/src/test/native/cpp/LoadConfigTest.cpp b/apriltag/src/test/native/cpp/LoadConfigTest.cpp index c3ff7c03c40..ba6fe81ff96 100644 --- a/apriltag/src/test/native/cpp/LoadConfigTest.cpp +++ b/apriltag/src/test/native/cpp/LoadConfigTest.cpp @@ -4,6 +4,7 @@ #include +#include "frc/apriltag/AprilTagFieldLayout.h" #include "frc/apriltag/AprilTagFields.h" namespace frc { @@ -20,7 +21,7 @@ std::vector GetAllFields() { TEST(AprilTagFieldsTest, TestLoad2022RapidReact) { AprilTagFieldLayout layout = - LoadAprilTagLayoutField(AprilTagField::k2022RapidReact); + AprilTagFieldLayout::LoadField(AprilTagField::k2022RapidReact); // Blue Hangar Truss - Hub auto expectedPose = @@ -53,7 +54,7 @@ class AllFieldsFixtureTest : public ::testing::TestWithParam {}; TEST_P(AllFieldsFixtureTest, CheckEntireEnum) { AprilTagField field = GetParam(); - EXPECT_NO_THROW(LoadAprilTagLayoutField(field)); + EXPECT_NO_THROW(AprilTagFieldLayout::LoadField(field)); } INSTANTIATE_TEST_SUITE_P(ValuesEnumTestInstTests, AllFieldsFixtureTest, diff --git a/glass/src/lib/native/cpp/other/FMS.cpp b/glass/src/lib/native/cpp/other/FMS.cpp index d49304e0396..4e702e5b481 100644 --- a/glass/src/lib/native/cpp/other/FMS.cpp +++ b/glass/src/lib/native/cpp/other/FMS.cpp @@ -5,6 +5,7 @@ #include "glass/other/FMS.h" #include +#include #include #include "glass/DataSource.h" @@ -58,8 +59,7 @@ void glass::DisplayFMS(FMSModel* model, bool editableDsAttached) { if (auto data = model->GetMatchTimeData()) { double val = data->GetValue(); ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8); - if (ImGui::InputDouble("Match Time", &val, 0, 0, "%.1f", - ImGuiInputTextFlags_EnterReturnsTrue)) { + if (ImGui::InputDouble("Match Time", &val, 0, 0, "%.1f")) { model->SetMatchTime(val); } data->EmitDrag(); @@ -78,16 +78,12 @@ void glass::DisplayFMS(FMSModel* model, bool editableDsAttached) { } // Game Specific Message - // make buffer full 64 width, null terminated, for editability - wpi::SmallString<64> gameSpecificMessage; - model->GetGameSpecificMessage(gameSpecificMessage); - gameSpecificMessage.resize(63); - gameSpecificMessage.push_back('\0'); + wpi::SmallString<64> gameSpecificMessageBuf; + std::string gameSpecificMessage{ + model->GetGameSpecificMessage(gameSpecificMessageBuf)}; ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8); - if (ImGui::InputText("Game Specific", gameSpecificMessage.data(), - gameSpecificMessage.size(), - ImGuiInputTextFlags_EnterReturnsTrue)) { - model->SetGameSpecificMessage(gameSpecificMessage.data()); + if (ImGui::InputText("Game Specific", &gameSpecificMessage)) { + model->SetGameSpecificMessage(gameSpecificMessage); } } @@ -151,9 +147,10 @@ void glass::DisplayFMSReadOnly(FMSModel* model) { } } - wpi::SmallString<64> gameSpecificMessage; - model->GetGameSpecificMessage(gameSpecificMessage); - ImGui::Text("Game Specific: %s", exists ? gameSpecificMessage.c_str() : "?"); + wpi::SmallString<64> gameSpecificMessageBuf; + std::string_view gameSpecificMessage = + model->GetGameSpecificMessage(gameSpecificMessageBuf); + ImGui::Text("Game Specific: %s", exists ? gameSpecificMessage.data() : "?"); if (!exists) { ImGui::PopStyleColor(); diff --git a/hal/src/main/java/edu/wpi/first/hal/HAL.java b/hal/src/main/java/edu/wpi/first/hal/HAL.java index d9eb8d599db..68cf0e8deed 100644 --- a/hal/src/main/java/edu/wpi/first/hal/HAL.java +++ b/hal/src/main/java/edu/wpi/first/hal/HAL.java @@ -75,6 +75,9 @@ public final class HAL extends JNIWrapper { */ public static native void exitMain(); + /** Terminates the executable (at the native level). Does nothing in simulation. */ + public static native void terminate(); + private static native void simPeriodicBeforeNative(); private static final List s_simPeriodicBefore = new ArrayList<>(); diff --git a/hal/src/main/native/athena/FRCDriverStation.cpp b/hal/src/main/native/athena/FRCDriverStation.cpp index c30fbb94c59..b0aeb05acc2 100644 --- a/hal/src/main/native/athena/FRCDriverStation.cpp +++ b/hal/src/main/native/athena/FRCDriverStation.cpp @@ -539,15 +539,25 @@ HAL_Bool HAL_RefreshDSData(void) { } // If newest state shows we have a DS attached, just use the // control word out of the cache, As it will be the one in sync - // with the data. Otherwise use the state that shows disconnected. - if (controlWord.dsAttached) { - newestControlWord = currentRead->controlWord; - } else { - // Zero out the control word. When the DS has never been connected - // this returns garbage. And there is no way we can detect that. - std::memset(&controlWord, 0, sizeof(controlWord)); - newestControlWord = controlWord; + // with the data. If no data has been updated, at this point, + // and a DS wasn't attached previously, this will still return + // a zeroed out control word, with is the correct state for + // no new data. + if (!controlWord.dsAttached) { + // If the DS is not attached, we need to zero out the control word. + // This is because HAL_RefreshDSData is called asynchronously from + // the DS data. The dsAttached variable comes directly from netcomm + // and could be updated before the caches are. If that happens, + // we would end up returning the previous cached control word, + // which is out of sync with the current control word and could + // break invariants such as which alliance station is in used. + // Also, when the DS has never been connected the rest of the fields + // in control word are garbage, so we also need to zero out in that + // case too + std::memset(¤tRead->controlWord, 0, + sizeof(currentRead->controlWord)); } + newestControlWord = currentRead->controlWord; } uint32_t mask = tcpMask.exchange(0); diff --git a/hal/src/main/native/athena/HAL.cpp b/hal/src/main/native/athena/HAL.cpp index f239391b360..2c57c9cea9e 100644 --- a/hal/src/main/native/athena/HAL.cpp +++ b/hal/src/main/native/athena/HAL.cpp @@ -524,7 +524,7 @@ static bool killExistingProgram(int timeout, int mode) { return true; } -static void SetupNowRio(void) { +static bool SetupNowRio(void) { nFPGA::nRoboRIO_FPGANamespace::g_currentTargetClass = nLoadOut::getTargetClass(); @@ -534,13 +534,13 @@ static void SetupNowRio(void) { status = dladdr(reinterpret_cast(tHMB::create), &info); if (status == 0) { fmt::print(stderr, "Failed to call dladdr on chipobject {}\n", dlerror()); - return; + return false; } void* chipObjectLibrary = dlopen(info.dli_fname, RTLD_LAZY); if (chipObjectLibrary == nullptr) { fmt::print(stderr, "Failed to call dlopen on chipobject {}\n", dlerror()); - return; + return false; } std::unique_ptr hmb; @@ -548,9 +548,9 @@ static void SetupNowRio(void) { if (hmb == nullptr) { fmt::print(stderr, "Failed to open HMB on chipobject {}\n", status); dlclose(chipObjectLibrary); - return; + return false; } - wpi::impl::SetupNowRio(chipObjectLibrary, std::move(hmb)); + return wpi::impl::SetupNowRio(chipObjectLibrary, std::move(hmb)); } HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) { @@ -593,7 +593,17 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) { setNewDataSem(nullptr); }); - SetupNowRio(); + if (!SetupNowRio()) { + fmt::print(stderr, + "Failed to run SetupNowRio(). This is a fatal error. The " + "process is being terminated.\n"); + std::fflush(stderr); + // Attempt to force a segfault to get a better java log + *reinterpret_cast(0) = 0; + // If that fails, terminate + std::terminate(); + return false; + } int32_t status = 0; diff --git a/hal/src/main/native/cpp/jni/HAL.cpp b/hal/src/main/native/cpp/jni/HAL.cpp index 6f486e5fc0f..9918f46d6ec 100644 --- a/hal/src/main/native/cpp/jni/HAL.cpp +++ b/hal/src/main/native/cpp/jni/HAL.cpp @@ -7,6 +7,7 @@ #include #include +#include #include #include @@ -82,6 +83,20 @@ Java_edu_wpi_first_hal_HAL_exitMain HAL_ExitMain(); } +/* + * Class: edu_wpi_first_hal_HAL + * Method: terminate + * Signature: ()V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_HAL_terminate + (JNIEnv*, jclass) +{ +#ifdef __FRC_ROBORIO__ + std::abort(); +#endif +} + /* * Class: edu_wpi_first_hal_HAL * Method: simPeriodicBeforeNative diff --git a/hal/src/main/native/sim/DriverStation.cpp b/hal/src/main/native/sim/DriverStation.cpp index b245eed59d5..2ad13de861b 100644 --- a/hal/src/main/native/sim/DriverStation.cpp +++ b/hal/src/main/native/sim/DriverStation.cpp @@ -44,6 +44,7 @@ struct JoystickDataCache { HAL_JoystickButtons buttons[kJoystickPorts]; HAL_AllianceStationID allianceStation; double matchTime; + HAL_ControlWord controlWord; }; static_assert(std::is_standard_layout_v); // static_assert(std::is_trivial_v); @@ -65,6 +66,16 @@ void JoystickDataCache::Update() { } allianceStation = SimDriverStationData->allianceStationId; matchTime = SimDriverStationData->matchTime; + + HAL_ControlWord tmpControlWord; + std::memset(&tmpControlWord, 0, sizeof(tmpControlWord)); + tmpControlWord.enabled = SimDriverStationData->enabled; + tmpControlWord.autonomous = SimDriverStationData->autonomous; + tmpControlWord.test = SimDriverStationData->test; + tmpControlWord.eStop = SimDriverStationData->eStop; + tmpControlWord.fmsAttached = SimDriverStationData->fmsAttached; + tmpControlWord.dsAttached = SimDriverStationData->dsAttached; + this->controlWord = tmpControlWord; } #define CHECK_JOYSTICK_NUMBER(stickNum) \ @@ -322,20 +333,32 @@ HAL_Bool HAL_RefreshDSData(void) { if (gShutdown) { return false; } - HAL_ControlWord controlWord; - std::memset(&controlWord, 0, sizeof(controlWord)); - controlWord.enabled = SimDriverStationData->enabled; - controlWord.autonomous = SimDriverStationData->autonomous; - controlWord.test = SimDriverStationData->test; - controlWord.eStop = SimDriverStationData->eStop; - controlWord.fmsAttached = SimDriverStationData->fmsAttached; - controlWord.dsAttached = SimDriverStationData->dsAttached; + bool dsAttached = SimDriverStationData->dsAttached; std::scoped_lock lock{driverStation->cacheMutex}; JoystickDataCache* prev = currentCache.exchange(nullptr); if (prev != nullptr) { currentRead = prev; } - newestControlWord = controlWord; + // If newest state shows we have a DS attached, just use the + // control word out of the cache, As it will be the one in sync + // with the data. If no data has been updated, at this point, + // and a DS wasn't attached previously, this will still return + // a zeroed out control word, with is the correct state for + // no new data. + if (!dsAttached) { + // If the DS is not attached, we need to zero out the control word. + // This is because HAL_RefreshDSData is called asynchronously from + // the DS data. The dsAttached variable comes directly from netcomm + // and could be updated before the caches are. If that happens, + // we would end up returning the previous cached control word, + // which is out of sync with the current control word and could + // break invariants such as which alliance station is in used. + // Also, when the DS has never been connected the rest of the fields + // in control word are garbage, so we also need to zero out in that + // case too + std::memset(¤tRead->controlWord, 0, sizeof(currentRead->controlWord)); + } + newestControlWord = currentRead->controlWord; return prev != nullptr; } @@ -369,6 +392,7 @@ void NewDriverStationData() { if (gShutdown) { return; } + SimDriverStationData->dsAttached = true; cacheToUpdate->Update(); JoystickDataCache* given = cacheToUpdate; @@ -382,7 +406,6 @@ void NewDriverStationData() { } lastGiven = given; - SimDriverStationData->dsAttached = true; driverStation->newDataEvents.Wakeup(); SimDriverStationData->CallNewDataCallbacks(); } diff --git a/ntcore/src/main/java/edu/wpi/first/networktables/PubSubOption.java b/ntcore/src/main/java/edu/wpi/first/networktables/PubSubOption.java index dd96eec8efd..b9d885533ee 100644 --- a/ntcore/src/main/java/edu/wpi/first/networktables/PubSubOption.java +++ b/ntcore/src/main/java/edu/wpi/first/networktables/PubSubOption.java @@ -15,7 +15,8 @@ enum Kind { disableRemote, disableLocal, excludePublisher, - excludeSelf + excludeSelf, + hidden } PubSubOption(Kind kind, boolean value) { @@ -149,6 +150,18 @@ public static PubSubOption excludeSelf(boolean enabled) { return new PubSubOption(Kind.excludeSelf, enabled); } + /** + * For subscriptions, don't share the existence of the subscription with the network. Note this + * means updates will not be received from the network unless another subscription overlaps with + * this one, and the subscription will not appear in metatopics. + * + * @param enabled True to enable, false to disable + * @return option + */ + public static PubSubOption hidden(boolean enabled) { + return new PubSubOption(Kind.hidden, enabled); + } + final Kind m_kind; final boolean m_bValue; final int m_iValue; diff --git a/ntcore/src/main/java/edu/wpi/first/networktables/PubSubOptions.java b/ntcore/src/main/java/edu/wpi/first/networktables/PubSubOptions.java index 18557b93db4..ec9158af392 100644 --- a/ntcore/src/main/java/edu/wpi/first/networktables/PubSubOptions.java +++ b/ntcore/src/main/java/edu/wpi/first/networktables/PubSubOptions.java @@ -42,6 +42,9 @@ public PubSubOptions(PubSubOption... options) { case excludeSelf: excludeSelf = option.m_bValue; break; + case hidden: + hidden = option.m_bValue; + break; default: break; } @@ -58,7 +61,8 @@ public PubSubOptions(PubSubOption... options) { boolean prefixMatch, boolean disableRemote, boolean disableLocal, - boolean excludeSelf) { + boolean excludeSelf, + boolean hidden) { this.pollStorage = pollStorage; this.periodic = periodic; this.excludePublisher = excludePublisher; @@ -69,6 +73,7 @@ public PubSubOptions(PubSubOption... options) { this.disableRemote = disableRemote; this.disableLocal = disableLocal; this.excludeSelf = excludeSelf; + this.hidden = hidden; } /** Default value of periodic. */ @@ -123,4 +128,11 @@ public PubSubOptions(PubSubOption... options) { /** For entries, don't queue (for readQueue) value updates for the entry's internal publisher. */ public boolean excludeSelf; + + /** + * For subscriptions, don't share the existence of the subscription with the network. Note this + * means updates will not be received from the network unless another subscription overlaps with + * this one, and the subscription will not appear in metatopics. + */ + public boolean hidden; } diff --git a/ntcore/src/main/native/cpp/LocalStorage.cpp b/ntcore/src/main/native/cpp/LocalStorage.cpp index 26e898a4838..7d1c3e4dc0d 100644 --- a/ntcore/src/main/native/cpp/LocalStorage.cpp +++ b/ntcore/src/main/native/cpp/LocalStorage.cpp @@ -611,7 +611,7 @@ LocalStorage::SubscriberData* LocalStorage::Impl::AddLocalSubscriber( "published as '{}')", topic->name, config.typeStr, topic->typeStr); } - if (m_network) { + if (m_network && !subscriber->config.hidden) { DEBUG4("-> NetworkSubscribe({})", topic->name); m_network->Subscribe(subscriber->handle, {{topic->name}}, config); } @@ -640,7 +640,7 @@ LocalStorage::Impl::RemoveLocalSubscriber(NT_Subscriber subHandle) { listener.getSecond()->subscriber = nullptr; } } - if (m_network) { + if (m_network && !subscriber->config.hidden) { m_network->Unsubscribe(subscriber->handle); } } @@ -676,7 +676,7 @@ LocalStorage::MultiSubscriberData* LocalStorage::Impl::AddMultiSubscriber( } } } - if (m_network) { + if (m_network && !subscriber->options.hidden) { DEBUG4("-> NetworkSubscribe"); m_network->Subscribe(subscriber->handle, subscriber->prefixes, subscriber->options); @@ -696,7 +696,7 @@ LocalStorage::Impl::RemoveMultiSubscriber(NT_MultiSubscriber subHandle) { listener.getSecond()->multiSubscriber = nullptr; } } - if (m_network) { + if (m_network && !subscriber->options.hidden) { m_network->Unsubscribe(subscriber->handle); } } @@ -1128,12 +1128,16 @@ void LocalStorage::Impl::StartNetwork(net::NetworkInterface* network) { } } for (auto&& subscriber : m_subscribers) { - network->Subscribe(subscriber->handle, {{subscriber->topic->name}}, - subscriber->config); + if (!subscriber->config.hidden) { + network->Subscribe(subscriber->handle, {{subscriber->topic->name}}, + subscriber->config); + } } for (auto&& subscriber : m_multiSubscribers) { - network->Subscribe(subscriber->handle, subscriber->prefixes, - subscriber->options); + if (!subscriber->options.hidden) { + network->Subscribe(subscriber->handle, subscriber->prefixes, + subscriber->options); + } } } diff --git a/ntcore/src/main/native/cpp/jni/NetworkTablesJNI.cpp b/ntcore/src/main/native/cpp/jni/NetworkTablesJNI.cpp index fdec2b90e35..ae10656bb99 100644 --- a/ntcore/src/main/native/cpp/jni/NetworkTablesJNI.cpp +++ b/ntcore/src/main/native/cpp/jni/NetworkTablesJNI.cpp @@ -139,6 +139,7 @@ static nt::PubSubOptions FromJavaPubSubOptions(JNIEnv* env, jobject joptions) { FIELD(disableRemote, "Z"); FIELD(disableLocal, "Z"); FIELD(excludeSelf, "Z"); + FIELD(hidden, "Z"); #undef FIELD @@ -154,7 +155,8 @@ static nt::PubSubOptions FromJavaPubSubOptions(JNIEnv* env, jobject joptions) { FIELD(bool, Boolean, prefixMatch), FIELD(bool, Boolean, disableRemote), FIELD(bool, Boolean, disableLocal), - FIELD(bool, Boolean, excludeSelf)}; + FIELD(bool, Boolean, excludeSelf), + FIELD(bool, Boolean, hidden)}; #undef GET #undef FIELD diff --git a/ntcore/src/main/native/cpp/net/ServerImpl.cpp b/ntcore/src/main/native/cpp/net/ServerImpl.cpp index 8dc5291a4ad..0a9071dd9d7 100644 --- a/ntcore/src/main/native/cpp/net/ServerImpl.cpp +++ b/ntcore/src/main/native/cpp/net/ServerImpl.cpp @@ -1816,7 +1816,8 @@ void ServerImpl::SetValue(ClientData* client, TopicData* topic, } for (auto&& tcd : topic->clients) { - if (tcd.second.sendMode != ValueSendMode::kDisabled) { + if (tcd.first != client && + tcd.second.sendMode != ValueSendMode::kDisabled) { tcd.first->SendValue(topic, value, tcd.second.sendMode); } } diff --git a/ntcore/src/main/native/cpp/ntcore_c.cpp b/ntcore/src/main/native/cpp/ntcore_c.cpp index c2561c8833d..cac7a3391eb 100644 --- a/ntcore/src/main/native/cpp/ntcore_c.cpp +++ b/ntcore/src/main/native/cpp/ntcore_c.cpp @@ -126,6 +126,7 @@ static PubSubOptions ConvertToCpp(const NT_PubSubOptions* in) { out.disableRemote = in->disableRemote; out.disableLocal = in->disableLocal; out.excludeSelf = in->excludeSelf; + out.hidden = in->hidden; return out; } diff --git a/ntcore/src/main/native/include/ntcore_c.h b/ntcore/src/main/native/include/ntcore_c.h index 6259a76e9d9..67efd766110 100644 --- a/ntcore/src/main/native/include/ntcore_c.h +++ b/ntcore/src/main/native/include/ntcore_c.h @@ -367,6 +367,14 @@ struct NT_PubSubOptions { * internal publisher. */ NT_Bool excludeSelf; + + /** + * For subscriptions, don't share the existence of the subscription with the + * network. Note this means updates will not be received from the network + * unless another subscription overlaps with this one, and the subscription + * will not appear in metatopics. + */ + NT_Bool hidden; }; /** diff --git a/ntcore/src/main/native/include/ntcore_cpp.h b/ntcore/src/main/native/include/ntcore_cpp.h index fe50f454376..f241f638118 100644 --- a/ntcore/src/main/native/include/ntcore_cpp.h +++ b/ntcore/src/main/native/include/ntcore_cpp.h @@ -374,6 +374,14 @@ struct PubSubOptions { * internal publisher. */ bool excludeSelf = false; + + /** + * For subscriptions, don't share the existence of the subscription with the + * network. Note this means updates will not be received from the network + * unless another subscription overlaps with this one, and the subscription + * will not appear in metatopics. + */ + bool hidden = false; }; /** diff --git a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp index 7f8a3765e3f..28096255e55 100644 --- a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp +++ b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp @@ -4,6 +4,8 @@ #include "sysid/analysis/FeedbackAnalysis.h" +#include + #include #include #include @@ -21,6 +23,10 @@ using Ka_t = decltype(1_V / 1_mps_sq); FeedbackGains sysid::CalculatePositionFeedbackGains( const FeedbackControllerPreset& preset, const LQRParameters& params, double Kv, double Ka) { + if (!std::isfinite(Kv) || !std::isfinite(Ka)) { + return {0.0, 0.0}; + } + // If acceleration requires no effort, velocity becomes an input for position // control. We choose an appropriate model in this case to avoid numerical // instabilities in the LQR. @@ -32,7 +38,8 @@ FeedbackGains sysid::CalculatePositionFeedbackGains( frc::LinearQuadraticRegulator<2, 1> controller{ system, {params.qp, params.qv}, {params.r}, preset.period}; // Compensate for any latency from sensor measurements, filtering, etc. - controller.LatencyCompensate(system, preset.period, 0.0_s); + controller.LatencyCompensate(system, preset.period, + preset.measurementDelay); return {controller.K(0, 0) * preset.outputConversionFactor, controller.K(0, 1) * preset.outputConversionFactor / @@ -47,7 +54,7 @@ FeedbackGains sysid::CalculatePositionFeedbackGains( frc::LinearQuadraticRegulator<1, 1> controller{ system, {params.qp}, {params.r}, preset.period}; // Compensate for any latency from sensor measurements, filtering, etc. - controller.LatencyCompensate(system, preset.period, 0.0_s); + controller.LatencyCompensate(system, preset.period, preset.measurementDelay); return {Kv * controller.K(0, 0) * preset.outputConversionFactor, 0.0}; } @@ -55,6 +62,10 @@ FeedbackGains sysid::CalculatePositionFeedbackGains( FeedbackGains sysid::CalculateVelocityFeedbackGains( const FeedbackControllerPreset& preset, const LQRParameters& params, double Kv, double Ka, double encFactor) { + if (!std::isfinite(Kv) || !std::isfinite(Ka)) { + return {0.0, 0.0}; + } + // If acceleration for velocity control requires no effort, the feedback // control gains approach zero. We special-case it here because numerical // instabilities arise in LQR otherwise. diff --git a/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp b/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp index db0381103aa..8432e7c8ff7 100644 --- a/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp +++ b/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp @@ -146,9 +146,18 @@ sysid::TrimStepVoltageData(std::vector* data, wpi::sgn(b.velocity) * b.acceleration; }); + // Current limiting can delay onset of the peak acceleration, so we need to + // find the first acceleration *near* the max. Magic number tolerance here + // because this whole file is tech debt already + auto accelBegins = std::find_if( + data->begin(), data->end(), [&maxAccel](const auto& measurement) { + return wpi::sgn(measurement.velocity) * measurement.acceleration > + 0.8 * wpi::sgn(maxAccel->velocity) * maxAccel->acceleration; + }); + units::second_t velocityDelay; - if (maxAccel != data->end()) { - velocityDelay = maxAccel->timestamp - firstTimestamp; + if (accelBegins != data->end()) { + velocityDelay = accelBegins->timestamp - firstTimestamp; // Trim data before max acceleration data->erase(data->begin(), maxAccel); diff --git a/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp b/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp index 44f664c800d..3e1474ef37e 100644 --- a/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp +++ b/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp @@ -103,3 +103,30 @@ TEST(FeedbackAnalysisTest, VelocityREVConversion) { EXPECT_NEAR(Kp, 0.00241 / 3, 0.005); EXPECT_NEAR(Kd, 0.00, 0.05); } + +TEST(FeedbackAnalysisTest, Position) { + auto Kv = 3.060; + auto Ka = 0.327; + + sysid::LQRParameters params{1, 1.5, 7}; + + auto [Kp, Kd] = sysid::CalculatePositionFeedbackGains( + sysid::presets::kDefault, params, Kv, Ka); + + EXPECT_NEAR(Kp, 6.41, 0.05); + EXPECT_NEAR(Kd, 2.48, 0.05); +} + +TEST(FeedbackAnalysisTest, PositionWithLatencyCompensation) { + auto Kv = 3.060; + auto Ka = 0.327; + + sysid::LQRParameters params{1, 1.5, 7}; + sysid::FeedbackControllerPreset preset{sysid::presets::kDefault}; + + preset.measurementDelay = 10_ms; + auto [Kp, Kd] = sysid::CalculatePositionFeedbackGains(preset, params, Kv, Ka); + + EXPECT_NEAR(Kp, 5.92, 0.05); + EXPECT_NEAR(Kd, 2.12, 0.05); +} diff --git a/vcpkg.json b/vcpkg.json index ea60ac26059..fc499d086bd 100644 --- a/vcpkg.json +++ b/vcpkg.json @@ -3,10 +3,9 @@ "version-string": "latest", "dependencies": [ "opencv", - "eigen3", "fmt", "libuv", "protobuf" ], - "builtin-baseline": "78b61582c9e093fda56a01ebb654be15a0033897" + "builtin-baseline": "37c3e63a1306562f7f59c4c3c8892ddd50fdf992" } diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java index 54d4edef5c3..44abdfc4db9 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java @@ -63,7 +63,7 @@ public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints) { @Override public void periodic() { - m_state = m_profile.calculate(m_period, m_goal, m_state); + m_state = m_profile.calculate(m_period, m_state, m_goal); if (m_enabled) { useState(m_state); } diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h index 344aefc77ea..56680103122 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h @@ -45,7 +45,7 @@ class TrapezoidProfileSubsystem : public SubsystemBase { m_period(period) {} void Periodic() override { - m_state = m_profile.Calculate(m_period, m_goal, m_state); + m_state = m_profile.Calculate(m_period, m_state, m_goal); if (m_enabled) { UseState(m_state); } diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggersTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggersTest.java index b2b41c4dfb6..f7df7f0bcb6 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggersTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggersTest.java @@ -6,7 +6,6 @@ import static org.junit.jupiter.api.Assertions.assertTrue; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.simulation.DriverStationSim; import edu.wpi.first.wpilibj2.command.CommandTestBase; import org.junit.jupiter.api.Test; @@ -18,7 +17,7 @@ void autonomousTest() { DriverStationSim.setAutonomous(true); DriverStationSim.setTest(false); DriverStationSim.setEnabled(true); - DriverStation.refreshData(); + DriverStationSim.notifyNewData(); Trigger auto = RobotModeTriggers.autonomous(); assertTrue(auto.getAsBoolean()); } @@ -29,7 +28,7 @@ void teleopTest() { DriverStationSim.setAutonomous(false); DriverStationSim.setTest(false); DriverStationSim.setEnabled(true); - DriverStation.refreshData(); + DriverStationSim.notifyNewData(); Trigger teleop = RobotModeTriggers.teleop(); assertTrue(teleop.getAsBoolean()); } @@ -40,7 +39,7 @@ void testModeTest() { DriverStationSim.setAutonomous(false); DriverStationSim.setTest(true); DriverStationSim.setEnabled(true); - DriverStation.refreshData(); + DriverStationSim.notifyNewData(); Trigger test = RobotModeTriggers.test(); assertTrue(test.getAsBoolean()); } @@ -51,7 +50,7 @@ void disabledTest() { DriverStationSim.setAutonomous(false); DriverStationSim.setTest(false); DriverStationSim.setEnabled(false); - DriverStation.refreshData(); + DriverStationSim.notifyNewData(); Trigger disabled = RobotModeTriggers.disabled(); assertTrue(disabled.getAsBoolean()); } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/button/RobotModeTriggersTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/button/RobotModeTriggersTest.cpp index cb9e9664ba0..adc9ec7732c 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/button/RobotModeTriggersTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/button/RobotModeTriggersTest.cpp @@ -18,7 +18,7 @@ TEST(RobotModeTriggersTest, Autonomous) { DriverStationSim::SetAutonomous(true); DriverStationSim::SetTest(false); DriverStationSim::SetEnabled(true); - frc::DriverStation::RefreshData(); + DriverStationSim::NotifyNewData(); Trigger autonomous = RobotModeTriggers::Autonomous(); EXPECT_TRUE(autonomous.Get()); } @@ -28,7 +28,7 @@ TEST(RobotModeTriggersTest, Teleop) { DriverStationSim::SetAutonomous(false); DriverStationSim::SetTest(false); DriverStationSim::SetEnabled(true); - frc::DriverStation::RefreshData(); + DriverStationSim::NotifyNewData(); Trigger teleop = RobotModeTriggers::Teleop(); EXPECT_TRUE(teleop.Get()); } @@ -38,7 +38,7 @@ TEST(RobotModeTriggersTest, Disabled) { DriverStationSim::SetAutonomous(false); DriverStationSim::SetTest(false); DriverStationSim::SetEnabled(false); - frc::DriverStation::RefreshData(); + DriverStationSim::NotifyNewData(); Trigger disabled = RobotModeTriggers::Disabled(); EXPECT_TRUE(disabled.Get()); } @@ -48,7 +48,7 @@ TEST(RobotModeTriggersTest, TestMode) { DriverStationSim::SetAutonomous(false); DriverStationSim::SetTest(true); DriverStationSim::SetEnabled(true); - frc::DriverStation::RefreshData(); + DriverStationSim::NotifyNewData(); Trigger test = RobotModeTriggers::Test(); EXPECT_TRUE(test.Get()); } diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h index f06c2524db6..0c17fdefe96 100644 --- a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h +++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h @@ -36,13 +36,12 @@ class SendableChooser : public SendableChooserBase { template static U _unwrap_smart_ptr(const U& value); - template - static U* _unwrap_smart_ptr(const std::unique_ptr& value); - template static std::weak_ptr _unwrap_smart_ptr(const std::shared_ptr& value); public: + using CopyType = decltype(_unwrap_smart_ptr(m_choices.lookup(""))); + SendableChooser() = default; ~SendableChooser() override = default; SendableChooser(SendableChooser&& rhs) = default; @@ -71,8 +70,8 @@ class SendableChooser : public SendableChooserBase { void SetDefaultOption(std::string_view name, T object); /** - * Returns a copy of the selected option (a raw pointer U* if T = - * std::unique_ptr or a std::weak_ptr if T = std::shared_ptr). + * Returns a copy of the selected option (a std::weak_ptr if T = + * std::shared_ptr). * * If there is none selected, it will return the default. If there is none * selected and no default, then it will return a value-initialized instance. @@ -81,7 +80,7 @@ class SendableChooser : public SendableChooserBase { * * @return The option selected */ - auto GetSelected() -> decltype(_unwrap_smart_ptr(m_choices[""])); + CopyType GetSelected() const; /** * Bind a listener that's called when the selected value changes. diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc index e40befdb74b..2b93629959c 100644 --- a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc +++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc @@ -33,8 +33,7 @@ void SendableChooser::SetDefaultOption(std::string_view name, T object) { template requires std::copy_constructible && std::default_initializable -auto SendableChooser::GetSelected() - -> decltype(_unwrap_smart_ptr(m_choices[""])) { +typename SendableChooser::CopyType SendableChooser::GetSelected() const { std::string selected = m_defaultChoice; { std::scoped_lock lock(m_mutex); @@ -43,9 +42,9 @@ auto SendableChooser::GetSelected() } } if (selected.empty()) { - return decltype(_unwrap_smart_ptr(m_choices[""])){}; + return CopyType{}; } else { - return _unwrap_smart_ptr(m_choices[selected]); + return _unwrap_smart_ptr(m_choices.lookup(selected)); } } @@ -121,13 +120,6 @@ U SendableChooser::_unwrap_smart_ptr(const U& value) { return value; } -template - requires std::copy_constructible && std::default_initializable -template -U* SendableChooser::_unwrap_smart_ptr(const std::unique_ptr& value) { - return value.get(); -} - template requires std::copy_constructible && std::default_initializable template diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooserBase.h b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooserBase.h index f0b4fedcbb0..3263f92f6b8 100644 --- a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooserBase.h +++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooserBase.h @@ -38,7 +38,7 @@ class SendableChooserBase : public wpi::Sendable, std::string m_defaultChoice; std::string m_selected; bool m_haveSelected = false; - wpi::mutex m_mutex; + mutable wpi::mutex m_mutex; int m_instance; std::string m_previousVal; static std::atomic_int s_instances; diff --git a/wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h b/wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h index 3c7544a6730..5e727169068 100644 --- a/wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h +++ b/wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h @@ -194,7 +194,7 @@ class SysIdRoutineLog { private: LogEntries m_logEntries; std::string m_logName; - bool m_stateInitialized; + bool m_stateInitialized = false; wpi::log::StringLogEntry m_state; }; } // namespace frc::sysid diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h index 63a9aea0142..633571ef173 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h @@ -131,7 +131,7 @@ class Drivetrain { nt::DoubleArrayEntry& m_cameraToObjectEntryRef = m_cameraToObjectEntry; frc::AprilTagFieldLayout m_aprilTagFieldLayout{ - frc::LoadAprilTagLayoutField(frc::AprilTagField::k2022RapidReact)}; + frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo)}; frc::Pose3d m_objectInField{m_aprilTagFieldLayout.GetTagPose(0).value()}; frc::PWMSparkMax m_leftLeader{1}; diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h index 80008c5d0ed..055680d9dcc 100644 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h +++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h @@ -44,7 +44,6 @@ class Elevator : public frc2::PIDSubsystem { private: frc::PWMSparkMax m_motor{5}; - double m_setpoint = 0; // Conversion value of potentiometer varies between the real world and // simulation diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h index 7df08b95ef5..7b99f175d0a 100644 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h +++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h @@ -41,7 +41,6 @@ class Wrist : public frc2::PIDSubsystem { private: frc::PWMSparkMax m_motor{6}; - double m_setpoint = 0; // Conversion value of potentiometer varies between the real world and // simulation diff --git a/wpilibcExamples/src/main/cpp/examples/SysId/cpp/SysIdRoutineBot.cpp b/wpilibcExamples/src/main/cpp/examples/SysId/cpp/SysIdRoutineBot.cpp index 45d480fe0b6..4e38374bff8 100644 --- a/wpilibcExamples/src/main/cpp/examples/SysId/cpp/SysIdRoutineBot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SysId/cpp/SysIdRoutineBot.cpp @@ -15,14 +15,28 @@ void SysIdRoutineBot::ConfigureBindings() { [this] { return -m_driverController.GetLeftY(); }, [this] { return -m_driverController.GetRightX(); })); - m_driverController.A().WhileTrue( - m_drive.SysIdQuasistatic(frc2::sysid::Direction::kForward)); - m_driverController.B().WhileTrue( - m_drive.SysIdQuasistatic(frc2::sysid::Direction::kReverse)); - m_driverController.X().WhileTrue( - m_drive.SysIdDynamic(frc2::sysid::Direction::kForward)); - m_driverController.Y().WhileTrue( - m_drive.SysIdDynamic(frc2::sysid::Direction::kReverse)); + // Using bumpers as a modifier and combining it with the buttons so that we + // can have both sets of bindings at once + (m_driverController.A() && m_driverController.RightBumper()) + .WhileTrue(m_drive.SysIdQuasistatic(frc2::sysid::Direction::kForward)); + (m_driverController.B() && m_driverController.RightBumper()) + .WhileTrue(m_drive.SysIdQuasistatic(frc2::sysid::Direction::kReverse)); + (m_driverController.X() && m_driverController.RightBumper()) + .WhileTrue(m_drive.SysIdDynamic(frc2::sysid::Direction::kForward)); + (m_driverController.Y() && m_driverController.RightBumper()) + .WhileTrue(m_drive.SysIdDynamic(frc2::sysid::Direction::kReverse)); + + m_shooter.SetDefaultCommand(m_shooter.RunShooterCommand( + [this] { return m_driverController.GetLeftTriggerAxis(); })); + + (m_driverController.A() && m_driverController.LeftBumper()) + .WhileTrue(m_shooter.SysIdQuasistatic(frc2::sysid::Direction::kForward)); + (m_driverController.B() && m_driverController.LeftBumper()) + .WhileTrue(m_shooter.SysIdQuasistatic(frc2::sysid::Direction::kReverse)); + (m_driverController.X() && m_driverController.LeftBumper()) + .WhileTrue(m_shooter.SysIdDynamic(frc2::sysid::Direction::kForward)); + (m_driverController.Y() && m_driverController.LeftBumper()) + .WhileTrue(m_shooter.SysIdDynamic(frc2::sysid::Direction::kReverse)); } frc2::CommandPtr SysIdRoutineBot::GetAutonomousCommand() { diff --git a/wpilibcExamples/src/main/cpp/examples/SysId/cpp/subsystems/Shooter.cpp b/wpilibcExamples/src/main/cpp/examples/SysId/cpp/subsystems/Shooter.cpp new file mode 100644 index 00000000000..8146cdd133b --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SysId/cpp/subsystems/Shooter.cpp @@ -0,0 +1,37 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "subsystems/Shooter.h" + +#include +#include +#include + +Shooter::Shooter() { + m_shooterEncoder.SetDistancePerPulse( + constants::shooter::kEncoderDistancePerPulse.value()); +} + +frc2::CommandPtr Shooter::RunShooterCommand( + std::function shooterSpeed) { + return frc2::cmd::Run( + [this, shooterSpeed] { + m_shooterMotor.SetVoltage( + units::volt_t{m_shooterFeedback.Calculate( + m_shooterEncoder.GetRate(), shooterSpeed())} + + m_shooterFeedforward.Calculate( + units::turns_per_second_t{shooterSpeed()})); + m_feederMotor.Set(constants::shooter::kFeederSpeed); + }, + {this}) + .WithName("Set Shooter Speed"); +} + +frc2::CommandPtr Shooter::SysIdQuasistatic(frc2::sysid::Direction direction) { + return m_sysIdRoutine.Quasistatic(direction); +} + +frc2::CommandPtr Shooter::SysIdDynamic(frc2::sysid::Direction direction) { + return m_sysIdRoutine.Dynamic(direction); +} diff --git a/wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h index e01f533afee..a0e7728c101 100644 --- a/wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/SysId/include/Constants.h @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -38,8 +39,13 @@ using kv_unit = units::inverse>; using kv_unit_t = units::unit_t; +using ka_unit = + units::compound_unit>; +using ka_unit_t = units::unit_t; + inline constexpr std::array kEncoderPorts = {4, 5}; -inline constexpr bool kLeftEncoderReversed = false; +inline constexpr bool kEncoderReversed = false; inline constexpr int kEncoderCpr = 1024; inline constexpr units::turn_t kEncoderDistancePerPulse = units::turn_t{1.0} / static_cast(kEncoderCpr); @@ -55,6 +61,7 @@ inline constexpr double kP = 1.0; inline constexpr units::volt_t kS = 0.05_V; inline constexpr kv_unit_t kV = (12_V) / kShooterFreeSpeed; +inline constexpr ka_unit_t kA = 0_V * 1_s * 1_s / units::turn_t{1}; inline constexpr double kFeederSpeed = 0.5; } // namespace shooter diff --git a/wpilibcExamples/src/main/cpp/examples/SysId/include/SysIdRoutineBot.h b/wpilibcExamples/src/main/cpp/examples/SysId/include/SysIdRoutineBot.h index 31110fc57a6..4363bb30ffc 100644 --- a/wpilibcExamples/src/main/cpp/examples/SysId/include/SysIdRoutineBot.h +++ b/wpilibcExamples/src/main/cpp/examples/SysId/include/SysIdRoutineBot.h @@ -9,6 +9,7 @@ #include "Constants.h" #include "subsystems/Drive.h" +#include "subsystems/Shooter.h" class SysIdRoutineBot { public: @@ -21,4 +22,5 @@ class SysIdRoutineBot { frc2::CommandXboxController m_driverController{ constants::oi::kDriverControllerPort}; Drive m_drive; + Shooter m_shooter; }; diff --git a/wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Shooter.h b/wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Shooter.h new file mode 100644 index 00000000000..0a33db68ea3 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/SysId/include/subsystems/Shooter.h @@ -0,0 +1,54 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" + +class Shooter : public frc2::SubsystemBase { + public: + Shooter(); + + frc2::CommandPtr RunShooterCommand(std::function shooterSpeed); + frc2::CommandPtr SysIdQuasistatic(frc2::sysid::Direction direction); + frc2::CommandPtr SysIdDynamic(frc2::sysid::Direction direction); + + private: + frc::PWMSparkMax m_shooterMotor{constants::shooter::kShooterMotorPort}; + frc::PWMSparkMax m_feederMotor{constants::shooter::kFeederMotorPort}; + + frc::Encoder m_shooterEncoder{constants::shooter::kEncoderPorts[0], + constants::shooter::kEncoderPorts[1], + constants::shooter::kEncoderReversed}; + + frc2::sysid::SysIdRoutine m_sysIdRoutine{ + frc2::sysid::Config{std::nullopt, std::nullopt, std::nullopt, + std::nullopt}, + frc2::sysid::Mechanism{ + [this](units::volt_t driveVoltage) { + m_shooterMotor.SetVoltage(driveVoltage); + }, + [this](frc::sysid::SysIdRoutineLog* log) { + log->Motor("shooter-wheel") + .voltage(m_shooterMotor.Get() * + frc::RobotController::GetBatteryVoltage()) + .position(units::turn_t{m_shooterEncoder.GetDistance()}) + .velocity( + units::turns_per_second_t{m_shooterEncoder.GetRate()}); + }, + this}}; + frc::PIDController m_shooterFeedback{constants::shooter::kP, 0, 0}; + frc::SimpleMotorFeedforward m_shooterFeedforward{ + constants::shooter::kS, constants::shooter::kV, constants::shooter::kA}; +}; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java index 89524063373..3c3035ce139 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java @@ -453,6 +453,11 @@ public static void startRobot(Supplier robotSupplier) { runRobot(robotSupplier); } + // On RIO, this will just terminate rather than shutting down cleanly (it's a no-op in sim). + // It's not worth the risk of hanging on shutdown when we want the code to restart as quickly + // as possible. + HAL.terminate(); + HAL.shutdown(); System.exit(0); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java index 9b7ec75b5f8..d0c200250b0 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java @@ -4,6 +4,7 @@ package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator; +import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.ComputerVisionUtil; import edu.wpi.first.math.VecBuilder; @@ -125,7 +126,8 @@ public Drivetrain(DoubleArrayTopic cameraToObjectTopic) { m_cameraToObjectEntry = cameraToObjectTopic.getEntry(m_defaultVal); - m_objectInField = AprilTagFields.k2022RapidReact.loadAprilTagLayoutField().getTagPose(0).get(); + m_objectInField = + AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo).getTagPose(0).get(); SmartDashboard.putData("Field", m_fieldSim); SmartDashboard.putData("FieldEstimation", m_fieldApproximation); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Constants.java index 285b98f4070..f7731317dec 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/Constants.java @@ -57,6 +57,7 @@ public static final class ShooterConstants { public static final double kVVoltSecondsPerRotation = // Should have value 12V at free speed... 12.0 / kShooterFreeRPS; + public static final double kAVoltSecondsSquaredPerRotation = 0; public static final double kFeederSpeed = 0.5; } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/SysIdRoutineBot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/SysIdRoutineBot.java index 6665f3f7cb0..93496fdcc37 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/SysIdRoutineBot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/SysIdRoutineBot.java @@ -4,9 +4,9 @@ package edu.wpi.first.wpilibj.examples.sysid; -import static edu.wpi.first.wpilibj.examples.sysid.Constants.OIConstants; - +import edu.wpi.first.wpilibj.examples.sysid.Constants.OIConstants; import edu.wpi.first.wpilibj.examples.sysid.subsystems.Drive; +import edu.wpi.first.wpilibj.examples.sysid.subsystems.Shooter; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -21,6 +21,7 @@ public class SysIdRoutineBot { // The robot's subsystems private final Drive m_drive = new Drive(); + private final Shooter m_shooter = new Shooter(); // The driver's controller CommandXboxController m_driverController = @@ -42,10 +43,44 @@ public void configureBindings() { // Bind full set of SysId routine tests to buttons; a complete routine should run each of these // once. - m_driverController.a().whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - m_driverController.b().whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - m_driverController.x().whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward)); - m_driverController.y().whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + // Using bumpers as a modifier and combining it with the buttons so that we can have both sets + // of bindings at once + m_driverController + .a() + .and(m_driverController.rightBumper()) + .whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + m_driverController + .b() + .and(m_driverController.rightBumper()) + .whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + m_driverController + .x() + .and(m_driverController.rightBumper()) + .whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward)); + m_driverController + .y() + .and(m_driverController.rightBumper()) + .whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + + // Control the shooter wheel with the left trigger + m_shooter.setDefaultCommand(m_shooter.runShooter(m_driverController::getLeftTriggerAxis)); + + m_driverController + .a() + .and(m_driverController.leftBumper()) + .whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + m_driverController + .b() + .and(m_driverController.leftBumper()) + .whileTrue(m_drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + m_driverController + .x() + .and(m_driverController.leftBumper()) + .whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward)); + m_driverController + .y() + .and(m_driverController.leftBumper()) + .whileTrue(m_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Drive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Drive.java index 3c9610897c5..9cad2950c0d 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Drive.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Drive.java @@ -122,10 +122,20 @@ public Command arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) { .withName("arcadeDrive"); } + /** + * Returns a command that will execute a quasistatic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { return m_sysIdRoutine.quasistatic(direction); } + /** + * Returns a command that will execute a dynamic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ public Command sysIdDynamic(SysIdRoutine.Direction direction) { return m_sysIdRoutine.dynamic(direction); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Shooter.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Shooter.java new file mode 100644 index 00000000000..322df8e1390 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Shooter.java @@ -0,0 +1,129 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.examples.sysid.subsystems; + +import static edu.wpi.first.units.MutableMeasure.mutable; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Volts; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.MutableMeasure; +import edu.wpi.first.units.Velocity; +import edu.wpi.first.units.Voltage; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.examples.sysid.Constants.ShooterConstants; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import java.util.function.DoubleSupplier; + +public class Shooter extends SubsystemBase { + // The motor on the shooter wheel . + private final PWMSparkMax m_shooterMotor = new PWMSparkMax(ShooterConstants.kShooterMotorPort); + + // The motor on the feeder wheels. + private final PWMSparkMax m_feederMotor = new PWMSparkMax(ShooterConstants.kFeederMotorPort); + + // The shooter wheel encoder + private final Encoder m_shooterEncoder = + new Encoder( + ShooterConstants.kEncoderPorts[0], + ShooterConstants.kEncoderPorts[1], + ShooterConstants.kEncoderReversed); + + // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. + private final MutableMeasure m_appliedVoltage = mutable(Volts.of(0)); + // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation. + private final MutableMeasure m_angle = mutable(Rotations.of(0)); + // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation. + private final MutableMeasure> m_velocity = mutable(RotationsPerSecond.of(0)); + + // Create a new SysId routine for characterizing the shooter. + private final SysIdRoutine m_sysIdRoutine = + new SysIdRoutine( + // Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage. + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + // Tell SysId how to plumb the driving voltage to the motor(s). + (Measure volts) -> { + m_shooterMotor.setVoltage(volts.in(Volts)); + }, + // Tell SysId how to record a frame of data for each motor on the mechanism being + // characterized. + log -> { + // Record a frame for the shooter motor. + log.motor("shooter-wheel") + .voltage( + m_appliedVoltage.mut_replace( + m_shooterMotor.get() * RobotController.getBatteryVoltage(), Volts)) + .angularPosition(m_angle.mut_replace(m_shooterEncoder.getDistance(), Rotations)) + .angularVelocity( + m_velocity.mut_replace(m_shooterEncoder.getRate(), RotationsPerSecond)); + }, + // Tell SysId to make generated commands require this subsystem, suffix test state in + // WPILog with this subsystem's name ("shooter") + this)); + // PID controller to run the shooter wheel in closed-loop, set the constants equal to those + // calculated by SysId + private final PIDController m_shooterFeedback = new PIDController(ShooterConstants.kP, 0, 0); + // Feedforward controller to run the shooter wheel in closed-loop, set the constants equal to + // those calculated by SysId + private final SimpleMotorFeedforward m_shooterFeedforward = + new SimpleMotorFeedforward( + ShooterConstants.kSVolts, + ShooterConstants.kVVoltSecondsPerRotation, + ShooterConstants.kAVoltSecondsSquaredPerRotation); + + /** Creates a new Shooter subsystem. */ + public Shooter() { + // Sets the distance per pulse for the encoders + m_shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse); + } + + /** + * Returns a command that runs the shooter at a specifc velocity. + * + * @param shooterSpeed The commanded shooter wheel speed in rotations per second + */ + public Command runShooter(DoubleSupplier shooterSpeed) { + // Run shooter wheel at the desired speed using a PID controller and feedforward. + return run(() -> { + m_shooterMotor.setVoltage( + m_shooterFeedback.calculate(m_shooterEncoder.getRate(), shooterSpeed.getAsDouble()) + + m_shooterFeedforward.calculate(shooterSpeed.getAsDouble())); + m_feederMotor.set(ShooterConstants.kFeederSpeed); + }) + .finallyDo( + () -> { + m_shooterMotor.stopMotor(); + m_feederMotor.stopMotor(); + }) + .withName("runShooter"); + } + + /** + * Returns a command that will execute a quasistatic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutine.quasistatic(direction); + } + + /** + * Returns a command that will execute a dynamic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutine.dynamic(direction); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/Vector.java b/wpimath/src/main/java/edu/wpi/first/math/Vector.java index bc467411259..8784a6c027b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/Vector.java +++ b/wpimath/src/main/java/edu/wpi/first/math/Vector.java @@ -5,6 +5,7 @@ package edu.wpi.first.math; import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import java.util.Objects; import org.ejml.simple.SimpleMatrix; @@ -48,6 +49,16 @@ public Vector(Matrix other) { super(other); } + /** + * Returns an element of the vector at a specified row. + * + * @param row The row that the element is located at. + * @return An element of the vector. + */ + public double get(int row) { + return get(row, 0); + } + @Override public Vector times(double value) { return new Vector<>(this.m_storage.scale(value)); @@ -105,12 +116,39 @@ public double dot(Vector other) { * @return The norm. */ public double norm() { - double squaredNorm = 0.0; + return Math.sqrt(dot(this)); + } - for (int i = 0; i < getNumRows(); ++i) { - squaredNorm += get(i, 0) * get(i, 0); - } + /** + * Returns the unit vector parallel with this vector. + * + * @return The unit vector. + */ + public Vector unit() { + return div(norm()); + } + + /** + * Returns the projection of this vector along another. + * + * @param other The vector to project along. + * @return The projection. + */ + public Vector projection(Vector other) { + return other.times(dot(other)).div(other.dot(other)); + } - return Math.sqrt(squaredNorm); + /** + * Returns the cross product of 3 dimensional vectors a and b. + * + * @param a The vector to cross with b. + * @param b The vector to cross with a. + * @return The cross product of a and b. + */ + public static Vector cross(Vector a, Vector b) { + return VecBuilder.fill( + a.get(1) * b.get(2) - a.get(2) * b.get(1), + a.get(2) * b.get(0) - a.get(0) * b.get(2), + a.get(0) * b.get(1) - a.get(1) * b.get(0)); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java index d7a695fced4..0d8b11b465f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java @@ -52,7 +52,7 @@ public ArmFeedforward(double ks, double kg, double kv, double ka) { throw new IllegalArgumentException("kv must be a non-negative number, got " + kv + "!"); } if (ka < 0.0) { - throw new IllegalArgumentException("ka must be a non-negative number, got " + kv + "!"); + throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!"); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index 854462a652c..dc56ece26a4 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java @@ -55,7 +55,7 @@ public ElevatorFeedforward(double ks, double kg, double kv, double ka) { throw new IllegalArgumentException("kv must be a non-negative number, got " + kv + "!"); } if (ka < 0.0) { - throw new IllegalArgumentException("ka must be a non-negative number, got " + kv + "!"); + throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!"); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java index 2b25bccbf8f..eeb76db3196 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java @@ -37,7 +37,7 @@ public SimpleMotorFeedforward(double ks, double kv, double ka) { throw new IllegalArgumentException("kv must be a non-negative number, got " + kv + "!"); } if (ka < 0.0) { - throw new IllegalArgumentException("ka must be a non-negative number, got " + kv + "!"); + throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!"); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java index c26270fa494..850b7f2f75c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java @@ -313,6 +313,15 @@ public double calculate(double input) { return retVal; } + /** + * Returns the last value calculated by the LinearFilter. + * + * @return The last value. + */ + public double lastValue() { + return m_outputs.getFirst(); + } + /** * Factorial of n. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/MedianFilter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/MedianFilter.java index 13c4e104373..d1d67b5b524 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/filter/MedianFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/filter/MedianFilter.java @@ -72,6 +72,15 @@ public double calculate(double next) { } } + /** + * Returns the last value calculated by the MedianFilter. + * + * @return The last value. + */ + public double lastValue() { + return m_valueBuffer.getFirst(); + } + /** Resets the filter, clearing the window of all elements. */ public void reset() { m_orderedValues.clear(); diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java index 6c34f35bc08..d9eca58419a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java @@ -64,6 +64,15 @@ public double calculate(double input) { return m_prevVal; } + /** + * Returns the value last calculated by the SlewRateLimiter. + * + * @return The last value. + */ + public double lastValue() { + return m_prevVal; + } + /** * Resets the slew rate limiter to the specified value; ignores the rate limit when doing so. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java index ea0be3cfdd6..5e1b581e54a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java @@ -285,11 +285,12 @@ public Quaternion log(Quaternion end) { * @return The logarithm of this quaternion. */ public Quaternion log() { - var scalar = Math.log(norm()); + var norm = norm(); + var scalar = Math.log(norm); var v_norm = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ()); - var s_norm = getW() / norm(); + var s_norm = getW() / norm; if (Math.abs(s_norm + 1) < 1e-9) { return new Quaternion(scalar, -Math.PI, 0, 0); diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java index f089f6690f3..4d04df354e3 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java @@ -11,9 +11,12 @@ import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.proto.Translation2dProto; import edu.wpi.first.math.geometry.struct.Translation2dStruct; import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.numbers.N2; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; import edu.wpi.first.util.protobuf.ProtobufSerializable; @@ -78,6 +81,16 @@ public Translation2d(Measure x, Measure y) { this(x.in(Meters), y.in(Meters)); } + /** + * Constructs a Translation2d from the provided translation vector's X and Y components. The + * values are assumed to be in meters. + * + * @param vector The translation vector to represent. + */ + public Translation2d(Vector vector) { + this(vector.get(0), vector.get(1)); + } + /** * Calculates the distance between two translations in 2D space. * @@ -110,6 +123,15 @@ public double getY() { return m_y; } + /** + * Returns a vector representation of this translation. + * + * @return A Vector representation of this translation. + */ + public Vector toVector() { + return VecBuilder.fill(m_x, m_y); + } + /** * Returns the norm, or distance from the origin to the translation. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java index 6d4fa9247e3..6068979fc38 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java @@ -11,9 +11,12 @@ import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.proto.Translation3dProto; import edu.wpi.first.math.geometry.struct.Translation3dStruct; import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; import edu.wpi.first.util.protobuf.ProtobufSerializable; @@ -83,6 +86,16 @@ public Translation3d(Measure x, Measure y, Measure this(x.in(Meters), y.in(Meters), z.in(Meters)); } + /** + * Constructs a Translation3d from the provided translation vector's X, Y, and Z components. The + * values are assumed to be in meters. + * + * @param vector The translation vector to represent. + */ + public Translation3d(Vector vector) { + this(vector.get(0), vector.get(1), vector.get(2)); + } + /** * Calculates the distance between two translations in 3D space. * @@ -126,6 +139,15 @@ public double getZ() { return m_z; } + /** + * Returns a vector representation of this translation. + * + * @return A Vector representation of this translation. + */ + public Vector toVector() { + return VecBuilder.fill(m_x, m_y, m_z); + } + /** * Returns the norm, or distance from the origin to the translation. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java index e2dae9322a0..76b7d6e764a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java @@ -191,7 +191,7 @@ public State calculate(double t, State current, State goal) { var timing = calculateProfileTiming(current, inflectionPoint, goal, u); if (t < 0) { - return current; + return new State(current.position, current.velocity); } else if (t < timing.inflectionTime) { return new State( computeDistanceFromTime(t, u, current), computeVelocityFromTime(t, u, current)); @@ -200,7 +200,7 @@ public State calculate(double t, State current, State goal) { computeDistanceFromTime(t - timing.totalTime, -u, goal), computeVelocityFromTime(t - timing.totalTime, -u, goal)); } else { - return goal; + return new State(goal.position, goal.velocity); } } diff --git a/wpimath/src/main/native/cpp/controller/RamseteController.cpp b/wpimath/src/main/native/cpp/controller/RamseteController.cpp index 576b0a9575d..d3bc708d97b 100644 --- a/wpimath/src/main/native/cpp/controller/RamseteController.cpp +++ b/wpimath/src/main/native/cpp/controller/RamseteController.cpp @@ -4,8 +4,6 @@ #include "frc/controller/RamseteController.h" -#include - #include "units/angle.h" #include "units/math.h" diff --git a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp index 90f7b491f5a..bd7ab8681e8 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp @@ -36,15 +36,15 @@ Eigen::Matrix3d RotationVectorToMatrix(const Eigen::Vector3d& rotation) { } // namespace Pose3d::Pose3d(Translation3d translation, Rotation3d rotation) - : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {} + : m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {} Pose3d::Pose3d(units::meter_t x, units::meter_t y, units::meter_t z, Rotation3d rotation) - : m_translation(x, y, z), m_rotation(std::move(rotation)) {} + : m_translation{x, y, z}, m_rotation{std::move(rotation)} {} Pose3d::Pose3d(const Pose2d& pose) - : m_translation(pose.X(), pose.Y(), 0_m), - m_rotation(0_rad, 0_rad, pose.Rotation().Radians()) {} + : m_translation{pose.X(), pose.Y(), 0_m}, + m_rotation{0_rad, 0_rad, pose.Rotation().Radians()} {} Pose3d Pose3d::operator+(const Transform3d& other) const { return TransformBy(other); diff --git a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp index ea9b179ca82..136f48db0c6 100644 --- a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp +++ b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp @@ -136,11 +136,12 @@ Quaternion Quaternion::Log(const Quaternion& other) const { } Quaternion Quaternion::Log() const { - double scalar = std::log(Norm()); + double norm = Norm(); + double scalar = std::log(norm); double v_norm = m_v.norm(); - double s_norm = W() / Norm(); + double s_norm = W() / norm; if (std::abs(s_norm + 1) < 1e-9) { return Quaternion{scalar, -std::numbers::pi, 0, 0}; diff --git a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp index 556a3027d0b..4879bb12ad5 100644 --- a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp @@ -19,11 +19,11 @@ Transform3d::Transform3d(Pose3d initial, Pose3d final) { } Transform3d::Transform3d(Translation3d translation, Rotation3d rotation) - : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {} + : m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {} Transform3d::Transform3d(units::meter_t x, units::meter_t y, units::meter_t z, Rotation3d rotation) - : m_translation(x, y, z), m_rotation(std::move(rotation)) {} + : m_translation{x, y, z}, m_rotation{std::move(rotation)} {} Transform3d Transform3d::Inverse() const { // We are rotating the difference between the translations diff --git a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp index c875582820e..90162018ed7 100644 --- a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp @@ -11,6 +11,9 @@ using namespace frc; +Translation2d::Translation2d(const Eigen::Vector2d& vector) + : m_x{units::meter_t{vector.x()}}, m_y{units::meter_t{vector.y()}} {} + units::meter_t Translation2d::Distance(const Translation2d& other) const { return units::math::hypot(other.m_x - m_x, other.m_y - m_y); } diff --git a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp index ecfee1ca3a2..c3b8cb4b775 100644 --- a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp @@ -19,6 +19,11 @@ Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) { m_z = rectangular.Z(); } +Translation3d::Translation3d(const Eigen::Vector3d& vector) + : m_x{units::meter_t{vector.x()}}, + m_y{units::meter_t{vector.y()}}, + m_z{units::meter_t{vector.z()}} {} + units::meter_t Translation3d::Distance(const Translation3d& other) const { return units::math::sqrt(units::math::pow<2>(other.m_x - m_x) + units::math::pow<2>(other.m_y - m_y) + diff --git a/wpimath/src/main/native/include/frc/filter/LinearFilter.h b/wpimath/src/main/native/include/frc/filter/LinearFilter.h index 051a6ddd268..3bc5465a3d2 100644 --- a/wpimath/src/main/native/include/frc/filter/LinearFilter.h +++ b/wpimath/src/main/native/include/frc/filter/LinearFilter.h @@ -371,6 +371,13 @@ class LinearFilter { return retVal; } + /** + * Returns the last value calculated by the LinearFilter. + * + * @return The last value. + */ + T LastValue() const { return m_outputs.front(); } + private: wpi::circular_buffer m_inputs; wpi::circular_buffer m_outputs; diff --git a/wpimath/src/main/native/include/frc/filter/MedianFilter.h b/wpimath/src/main/native/include/frc/filter/MedianFilter.h index 40422a6d24a..4c714fb547e 100644 --- a/wpimath/src/main/native/include/frc/filter/MedianFilter.h +++ b/wpimath/src/main/native/include/frc/filter/MedianFilter.h @@ -61,6 +61,13 @@ class MedianFilter { } } + /** + * Returns the last value calculated by the MedianFilter. + * + * @return The last value. + */ + T LastValue() const { return m_valueBuffer.front(); } + /** * Resets the filter, clearing the window of all elements. */ diff --git a/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h b/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h index 9f7c6744b97..ceba4ca2bc7 100644 --- a/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h +++ b/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h @@ -75,6 +75,13 @@ class SlewRateLimiter { return m_prevVal; } + /** + * Returns the value last calculated by the SlewRateLimiter. + * + * @return The last value. + */ + Unit_t LastValue() const { return m_prevVal; } + /** * Resets the slew rate limiter to the specified value; ignores the rate limit * when doing so. diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.inc b/wpimath/src/main/native/include/frc/geometry/Pose2d.inc index 2764d16cbf8..559a0034e80 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose2d.inc +++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.inc @@ -13,11 +13,11 @@ namespace frc { constexpr Pose2d::Pose2d(Translation2d translation, Rotation2d rotation) - : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {} + : m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {} constexpr Pose2d::Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation) - : m_translation(x, y), m_rotation(std::move(rotation)) {} + : m_translation{x, y}, m_rotation{std::move(rotation)} {} constexpr Pose2d Pose2d::operator+(const Transform2d& other) const { return TransformBy(other); diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc b/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc index a222b362317..104b66b1605 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc +++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc @@ -12,9 +12,9 @@ namespace frc { constexpr Rotation2d::Rotation2d(units::angle_unit auto value) - : m_value(value), - m_cos(gcem::cos(value.template convert().value())), - m_sin(gcem::sin(value.template convert().value())) {} + : m_value{value}, + m_cos{gcem::cos(value.template convert().value())}, + m_sin{gcem::sin(value.template convert().value())} {} constexpr Rotation2d::Rotation2d(double x, double y) { double magnitude = gcem::hypot(x, y); @@ -33,7 +33,7 @@ constexpr Rotation2d Rotation2d::operator-() const { } constexpr Rotation2d Rotation2d::operator*(double scalar) const { - return Rotation2d(m_value * scalar); + return Rotation2d{m_value * scalar}; } constexpr Rotation2d Rotation2d::operator+(const Rotation2d& other) const { diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.inc b/wpimath/src/main/native/include/frc/geometry/Transform2d.inc index 862b8d5c600..cc925148d45 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform2d.inc +++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.inc @@ -14,11 +14,11 @@ namespace frc { constexpr Transform2d::Transform2d(Translation2d translation, Rotation2d rotation) - : m_translation(std::move(translation)), m_rotation(std::move(rotation)) {} + : m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {} constexpr Transform2d::Transform2d(units::meter_t x, units::meter_t y, Rotation2d rotation) - : m_translation(x, y), m_rotation(std::move(rotation)) {} + : m_translation{x, y}, m_rotation{std::move(rotation)} {} constexpr Transform2d Transform2d::Inverse() const { // We are rotating the difference between the translations diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h index a99492dcebd..9d9a77c3bf2 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h @@ -7,6 +7,7 @@ #include #include +#include #include #include @@ -48,6 +49,14 @@ class WPILIB_DLLEXPORT Translation2d { */ constexpr Translation2d(units::meter_t distance, const Rotation2d& angle); + /** + * Constructs a Translation2d from the provided translation vector's X and Y + * components. The values are assumed to be in meters. + * + * @param vector The translation vector to represent. + */ + explicit Translation2d(const Eigen::Vector2d& vector); + /** * Calculates the distance between two translations in 2D space. * @@ -73,6 +82,13 @@ class WPILIB_DLLEXPORT Translation2d { */ constexpr units::meter_t Y() const { return m_y; } + /** + * Returns a vector representation of this translation. + * + * @return A Vector representation of this translation. + */ + constexpr Eigen::Vector2d ToVector() const; + /** * Returns the norm, or distance from the origin to the translation. * diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.inc b/wpimath/src/main/native/include/frc/geometry/Translation2d.inc index 3be897fad6d..6b291e1f74c 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation2d.inc +++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.inc @@ -10,11 +10,15 @@ namespace frc { constexpr Translation2d::Translation2d(units::meter_t x, units::meter_t y) - : m_x(x), m_y(y) {} + : m_x{x}, m_y{y} {} constexpr Translation2d::Translation2d(units::meter_t distance, const Rotation2d& angle) - : m_x(distance * angle.Cos()), m_y(distance * angle.Sin()) {} + : m_x{distance * angle.Cos()}, m_y{distance * angle.Sin()} {} + +constexpr Eigen::Vector2d Translation2d::ToVector() const { + return Eigen::Vector2d{{m_x.value(), m_y.value()}}; +} constexpr Rotation2d Translation2d::Angle() const { return Rotation2d{m_x.value(), m_y.value()}; diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h index b0b3359d96e..b83b661fd98 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h @@ -4,6 +4,7 @@ #pragma once +#include #include #include @@ -47,6 +48,14 @@ class WPILIB_DLLEXPORT Translation3d { */ Translation3d(units::meter_t distance, const Rotation3d& angle); + /** + * Constructs a Translation3d from the provided translation vector's X, Y, and + * Z components. The values are assumed to be in meters. + * + * @param vector The translation vector to represent. + */ + explicit Translation3d(const Eigen::Vector3d& vector); + /** * Calculates the distance between two translations in 3D space. * @@ -80,6 +89,13 @@ class WPILIB_DLLEXPORT Translation3d { */ constexpr units::meter_t Z() const { return m_z; } + /** + * Returns a vector representation of this translation. + * + * @return A Vector representation of this translation. + */ + constexpr Eigen::Vector3d ToVector() const; + /** * Returns the norm, or distance from the origin to the translation. * diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.inc b/wpimath/src/main/native/include/frc/geometry/Translation3d.inc index 8ab6e94c609..19268e6cee2 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation3d.inc +++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.inc @@ -13,12 +13,16 @@ namespace frc { constexpr Translation3d::Translation3d(units::meter_t x, units::meter_t y, units::meter_t z) - : m_x(x), m_y(y), m_z(z) {} + : m_x{x}, m_y{y}, m_z{z} {} constexpr Translation2d Translation3d::ToTranslation2d() const { return Translation2d{m_x, m_y}; } +constexpr Eigen::Vector3d Translation3d::ToVector() const { + return Eigen::Vector3d{{m_x.value(), m_y.value(), m_z.value()}}; +} + constexpr Translation3d Translation3d::operator+( const Translation3d& other) const { return {X() + other.X(), Y() + other.Y(), Z() + other.Z()}; diff --git a/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java index 008953f9170..7b6edefe662 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/VectorTest.java @@ -65,4 +65,40 @@ void testVectorNorm() { assertEquals(Math.sqrt(14.0), VecBuilder.fill(1.0, 2.0, 3.0).norm()); assertEquals(Math.sqrt(14.0), VecBuilder.fill(1.0, -2.0, 3.0).norm()); } + + @Test + void testVectorUnit() { + assertEquals(VecBuilder.fill(3.0, 4.0).unit(), VecBuilder.fill(3.0 / 5.0, 4.0 / 5.0)); + assertEquals(VecBuilder.fill(8.0, 15.0).unit(), VecBuilder.fill(8.0 / 17.0, 15.0 / 17.0)); + } + + @Test + void testVectorProjection() { + var vec1 = VecBuilder.fill(1.0, 2.0, 3.0); + var vec2 = VecBuilder.fill(4.0, 5.0, 6.0); + var res1 = vec1.projection(vec2); + + assertEquals(res1.get(0), 1.662, 0.01); + assertEquals(res1.get(1), 2.077, 0.01); + assertEquals(res1.get(2), 2.49, 0.01); + + var vec3 = VecBuilder.fill(-1.0, 2.0, -3.0); + var vec4 = VecBuilder.fill(4.0, -5.0, 6.0); + var res2 = vec4.projection(vec3); + + assertEquals(res2.get(0), 2.29, 0.01); + assertEquals(res2.get(1), -4.57, 0.01); + assertEquals(res2.get(2), 6.86, 0.01); + } + + @Test + void testVectorCross() { + var e1 = VecBuilder.fill(1.0, 0.0, 0.0); + var e2 = VecBuilder.fill(0.0, 1.0, 0.0); + assertEquals(Vector.cross(e1, e2), VecBuilder.fill(0.0, 0.0, 1.0)); + + var vec1 = VecBuilder.fill(1.0, 2.0, 3.0); + var vec2 = VecBuilder.fill(3.0, 4.0, 5.0); + assertEquals(Vector.cross(vec1, vec2), VecBuilder.fill(-2.0, 4.0, -2.0)); + } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java index 6ed8a61478b..39e2073b638 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java @@ -8,6 +8,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotEquals; +import edu.wpi.first.math.VecBuilder; import java.util.List; import org.junit.jupiter.api.Test; @@ -131,4 +132,15 @@ void testNearest() { assertEquals(origin.nearest(List.of(translation1, translation2, translation3)), translation1); assertEquals(origin.nearest(List.of(translation4, translation2, translation3)), translation2); } + + @Test + void testToVector() { + var vec = VecBuilder.fill(1.0, 2.0); + var translation = new Translation2d(vec); + + assertEquals(vec.get(0), translation.getX()); + assertEquals(vec.get(1), translation.getY()); + + assertEquals(vec, translation.toVector()); + } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java index cd68dbec77d..3dd54227ea8 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java @@ -160,4 +160,16 @@ void testPolarConstructor() { () -> assertEquals(Math.sqrt(3.0), two.getY(), kEpsilon), () -> assertEquals(0.0, two.getZ(), kEpsilon)); } + + @Test + void testToVector() { + var vec = VecBuilder.fill(1.0, 2.0, 3.0); + var translation = new Translation3d(vec); + + assertEquals(vec.get(0), translation.getX()); + assertEquals(vec.get(1), translation.getY()); + assertEquals(vec.get(2), translation.getZ()); + + assertEquals(vec, translation.toVector()); + } } diff --git a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp index 1c92c0a8b8c..46d383ea584 100644 --- a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp @@ -126,6 +126,16 @@ TEST(Translation2dTest, Nearest) { translation2.Y().value()); } +TEST(Translation2dTest, ToVector) { + const Eigen::Vector2d vec(1.0, 2.0); + const Translation2d translation{vec}; + + EXPECT_DOUBLE_EQ(vec[0], translation.X().value()); + EXPECT_DOUBLE_EQ(vec[1], translation.Y().value()); + + EXPECT_TRUE(vec == translation.ToVector()); +} + TEST(Translation2dTest, Constexpr) { constexpr Translation2d defaultCtor; constexpr Translation2d componentCtor{1_m, 2_m}; diff --git a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp index b7aa8ceeff6..56eaf6da753 100644 --- a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp @@ -128,6 +128,17 @@ TEST(Translation3dTest, PolarConstructor) { EXPECT_NEAR(two.Z().value(), 0.0, kEpsilon); } +TEST(Translation3dTest, ToVector) { + const Eigen::Vector3d vec(1.0, 2.0, 3.0); + const Translation3d translation{vec}; + + EXPECT_DOUBLE_EQ(vec[0], translation.X().value()); + EXPECT_DOUBLE_EQ(vec[1], translation.Y().value()); + EXPECT_DOUBLE_EQ(vec[2], translation.Z().value()); + + EXPECT_TRUE(vec == translation.ToVector()); +} + TEST(Translation3dTest, Constexpr) { constexpr Translation3d defaultCtor; constexpr Translation3d componentCtor{1_m, 2_m, 3_m}; diff --git a/wpinet/build.gradle b/wpinet/build.gradle index b8b86e49c4b..b1a5019a8bd 100644 --- a/wpinet/build.gradle +++ b/wpinet/build.gradle @@ -206,7 +206,7 @@ cppHeadersZip { from('src/main/native/thirdparty/libuv/include') { into '/' } - from('src/main/native/third_party/tcpsockets/include') { + from('src/main/native/thirdparty/tcpsockets/include') { into '/' } } diff --git a/wpiunits/src/main/java/edu/wpi/first/units/Distance.java b/wpiunits/src/main/java/edu/wpi/first/units/Distance.java index 2e324191419..7a0336f8442 100644 --- a/wpiunits/src/main/java/edu/wpi/first/units/Distance.java +++ b/wpiunits/src/main/java/edu/wpi/first/units/Distance.java @@ -5,10 +5,10 @@ package edu.wpi.first.units; /** - * Unit of angular dimension. + * Unit of linear dimension. * - *

This is the base type for units of distance dimension. It is also used to specify the - * dimension for {@link Measure}: Measure<Distance>. + *

This is the base type for units of linear dimension. It is also used to specify the dimension + * for {@link Measure}: Measure<Distance>. * *

Actual units (such as {@link Units#Meters} and {@link Units#Inches}) can be found in the * {@link Units} class. diff --git a/wpiutil/src/main/native/cpp/timestamp.cpp b/wpiutil/src/main/native/cpp/timestamp.cpp index c811964afd6..47ac70fe597 100644 --- a/wpiutil/src/main/native/cpp/timestamp.cpp +++ b/wpiutil/src/main/native/cpp/timestamp.cpp @@ -133,13 +133,15 @@ struct HMBLowLevel { dlcloseWrapper}; }; struct HMBHolder { - void Configure(void* col, std::unique_ptr hmbObject) { + bool Configure(void* col, std::unique_ptr hmbObject) { hmb = std::move(hmbObject); chipObjectLibrary.reset(col); if (!lowLevel.Configure(hmb->getSystemInterface()->getHandle())) { hmb = nullptr; chipObjectLibrary = nullptr; + return false; } + return true; } void Reset() { lowLevel.Reset(); @@ -236,20 +238,22 @@ void wpi::impl::SetupNowDefaultOnRio() { #ifdef __FRC_ROBORIO__ template <> -void wpi::impl::SetupNowRio(void* chipObjectLibrary, +bool wpi::impl::SetupNowRio(void* chipObjectLibrary, std::unique_ptr hmbObject) { if (!hmbInitialized.test()) { - hmb.Configure(chipObjectLibrary, std::move(hmbObject)); + return hmb.Configure(chipObjectLibrary, std::move(hmbObject)); } + return true; } #endif -void wpi::impl::SetupNowRio(uint32_t session) { +bool wpi::impl::SetupNowRio(uint32_t session) { #ifdef __FRC_ROBORIO__ if (!hmbInitialized.test()) { - hmb.lowLevel.Configure(session); + return hmb.lowLevel.Configure(session); } #endif + return true; } void wpi::impl::ShutdownNowRio() { @@ -269,10 +273,14 @@ uint64_t wpi::Now() { if (nowUseDefaultOnFailure.test()) { return timestamp() - offset_val; } else { - fmt::print( - stderr, - "FPGA not yet configured in wpi::Now(). Time will not be correct.\n"); + fmt::print(stderr, + "FPGA not yet configured in wpi::Now(). This is a fatal " + "error. The process is being terminated.\n"); std::fflush(stderr); + // Attempt to force a segfault to get a better java log + *reinterpret_cast(0) = 0; + // If that fails, terminate + std::terminate(); return 1; } } @@ -311,7 +319,7 @@ void WPI_Impl_SetupNowUseDefaultOnRio(void) { } void WPI_Impl_SetupNowRioWithSession(uint32_t session) { - return wpi::impl::SetupNowRio(session); + wpi::impl::SetupNowRio(session); } void WPI_Impl_ShutdownNowRio(void) { diff --git a/wpiutil/src/main/native/include/wpi/timestamp.h b/wpiutil/src/main/native/include/wpi/timestamp.h index 4d61055d3c2..26b586f26e0 100644 --- a/wpiutil/src/main/native/include/wpi/timestamp.h +++ b/wpiutil/src/main/native/include/wpi/timestamp.h @@ -74,10 +74,12 @@ void SetupNowDefaultOnRio(); */ #ifdef __FRC_ROBORIO__ template -void SetupNowRio(void* chipObjectLibrary, std::unique_ptr hmbObject); +bool SetupNowRio(void* chipObjectLibrary, std::unique_ptr hmbObject); #else template -inline void SetupNowRio(void*, std::unique_ptr) {} +inline bool SetupNowRio(void*, std::unique_ptr) { + return true; +} #endif /** @@ -85,7 +87,7 @@ inline void SetupNowRio(void*, std::unique_ptr) {} * No effect on non-Rio platforms. This take an FPGA session that has * already been initialized, and is used from LabVIEW. */ -void SetupNowRio(uint32_t session); +bool SetupNowRio(uint32_t session); /** * De-initialize the on-Rio Now() implementation. No effect on non-Rio